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ABSTRACT 


This  study  investigates  the  utility  of  rule-based  coordination  of  motion  for  ternary-type  terrain  locomotion 


by  a  hexapod  walking  machine. ,  The  ternary-type  terrain  considered  is  composed  of  permitted  areas, 
forbidden  areas,  and  ditch  areas.  The  logic  for  generating  motion  coordination  is  written  in  Prolog  while  the 
simulation  of  the  terrain  and  of  the  vehicle  kinematics,  as  well  as  low-level  on-board  computer  functions, 
are  written  in  extended  Common  Lisp  and  Flavors.  It  is  found  that  this  approach,  which  utilizes  multiple 
programming  paradigms  for  programming  motion  coordination  logic  and  simulation  objects,  results  in  code 
that  is  much  easier  to  understand  and  modify  than  previous  motion  coordination  programs  written  in  Pascal. 
Thus,  the  code  development  effort  and  time  are  greatly  reduced.  The  authors  believe  that  both  the 


methodology  and  the  motion  coordination  logic  presented  in  this  report  possess  sufficient  merit  to  justify 


full-scale  physical  testing  in  the  Adaptive  Suspension  Vehicle  at  the  Ohio  State  University. 
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1.  Introduction 


The  Adaptive  Suspension  Vehicle  (AS  V)  is  a  large  six-legged  vehicle  designed  for  outdoor 
operation  in  rough  terrain.  Limb  motion  coordination  for  the  ASV  is  accomplished  by  an  on-board 
computer  network  consisting  of  one  PC-AT,  eight  Intel  single-board  computers,  and  two  special  purpose 
computers  [1,2],  The  software  system  is  hierarchically  organized  with  a  clear  distinction  being  made  among 
an  individual  leg  control  level,  a  leg  motion  coordination  level,  and  a  body  motion  planning  level  [2,3]. 
Except  for  the  two  special  purpose  computers,  the  application  software  for  the  ASV  is  currently  written 
almost  entirely  in  Pascal.  A  custom  designed  real-time  operating  system,  written  mainly  in  PL/M, 
coordinates  the  functioning  of  all  processes  running  on  the  various  processors  of  the  vehicle  computer.  The 
total  ASV  software  system  involves  somewhat  more  than  150,000  lines  of  code  [2,4]. 

An  important  feature  of  the  ASV  is  its  omni-directional  motion  capability  [1,2]  which  gives  it  the 
general  maneuverability  characteristics  of  a  helicopter.  This  behavior  is  achieved  by  providing  the  operator 
with  a  joystick  with  three  major  motion  axes  for  control  of  vehicle  forward  velocity,  lateral  velocity,  and 
turning  velocity  respectively  [2],  The  vehicle  control  computer  accepts  these  commands  and  synthesizes  a 
sequence  of  leg  movements  to  produce  the  desired  body  behavior.  It  is  assisted  in  this  task  by  information 
from  an  optical  terrain  scanner  which  provides  a  map  of  terrain  elevation  in  the  immediate  vicinity  of  the 
vehicle  [S],  and  by  force  and  position  feedback  from  each  leg. 

Until  now,  nearly  all  outdoor  experiments  with  the  ASV  have  made  use  of  a  tripod  gait  in  which 
legs  are  used  in  two  sets  of  overlapping  tripods  [6,7],  This  gait  was  chosen  both  for  its  relative  simplicity 
and  for  its  known  optimality  under  high  speed  straight-line  locomotion  conditions  [7,8,9],  However,  the 
tripod  gait  is  not  well  suited  to  extreme  terrain  situations  in  which  a  significant  fraction  of  the  area  under  a 
given  leg  may  be  unsatisfactory  for  load  bearing  due  to  the  presence  of  rocks,  holes,  obstacles,  soft  soils, 
etc.  In  the  latter  case,  simulation  experiments  [10 ,11],  and  initial  indoor  testing  [12],  indicate  that  on-line 
optimization  of  leg  sequencing  should  give  better  results. 

Gaits  involving  real-time  optimization  of  stability  or  maneuverability  in  the  presence  of  terrain 
constraints  are  often  called  free  gaits  to  distinguish  them  from  the  periodic  gaits  used  by  walking  machines 
and  animals  in  less  difficult  circumstances  [13,14],  Until  now,  all  free  gait  studies  have  been  performed 
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based  on  a  binary  terrain  model  which  includes  two  types  of  tenain  objects;  i.e.,  obstacle  and  non-obstacle 
[10,11,15,16] .  The  size  of  the  individual  obstacles  is  restricted  to  be  comparable  to  that  of  the  feet  of  the 
AS V  with  the  further  assumption  that  the  obstacles  are  randomly  distributed  on  the  tenain.  In  this  study, 
in  addition  to  the  above  small  obstacles,  a  ditch  obstacle,  which  is  a  large  and  structured  obstacle  whose  size 
is  comparable  to  that  of  the  vehicle,  is  introduced  as  a  third  type.  Therefore,  the  terrain  model  is  ternary 
rather  than  binary,  and  contains  both  randomly  distributed  small  obstacles  and  large  ditch  obstacles. 

Though  the  free  gait  motion  coordinator  developed  in  [16]  performs  well  ior  binary  terrain,  in  this  report,  a 
new  motion  coordinator,  called  the  "Ditch  Crossing  Motion  Coordinator"  is  additionally  introduced  to 
enhance  ditch  crossing  capability  Thus,  two  motion  coordinators  coexist  in  the  program,  while,  at  one 
instance,  only  one  of  them  is  allowed  to  control  the  vehicle  depending  on  terrain  conditions.  If  the 
existence  of  a  ditch  is  detected,  the  newly  introduced  motion  coordinator  takes  over  control  of  the  vehicle 
until  the  AS  V  has  crossed  the  ditch.  After  crossing  the  ditch,  vehicle  control  is  automatically  transferred 
back  to  the  Free  Gait  Motion  Coordinator  developed  in  [16]  to  handle  small  obstacles  effectively. 

Differing  from  all  AS  V  experiments  using  an  imperative  language  (Pascal)  to  encode  stepping 
algorithms,  this  study  uses  Prolog,  Flavors  [18],  and  Lisp  as  in  [16,17]  because  of  the  authors'  belief  that 
such  a  multiple  programming  paradigm  is  very  well  suited  to  the  complex  nature  of  the  motion 
coordination  problem  for  the  ASV,  involving  a  constant  interaction  among  the  on-line  optimization  logic, 
the  vehicle  and  its  internal  objects,  and  the  numerical  routines.  In  what  follows,  Prolog  is  used  to  program 
the  first  pa/t,  and  Flavors  and  Lisp  are  introduced  to  program  the  second  and  the  third  parts.  This  division 
of  the  motion  coordination  problem  is  suggested  by  the  physiology  of  animals  which  utilize  a  brain,  a 
physical  body,  and  muscles  to  move  themselves.  Moreover,  like  the  hierarchical  organization  existing 
among  the  these  functional  parts  of  animals,  the  program  described  in  this  report  is  also  organized  according 
to  same  type  of  hierarchy.  Specifically,  the  top  level  of  the  program  is  the  motion  coordinator  written  in 
Prolog,  while  the  second  level  is  the  Flavor  objects  which  simulate  a  body,  legs,  and  sensory  organs.  The 
numerical  function  routines  written  in  Lisp  perform  ail  necessary  calculations  to  move  the  legs  and  the 
body  analogous  to  the  action  of  muscles.  The  resulting  code  is  remarkably  easy  to  understand  and  modify 
because  each  programming  language  naturally  provides  the  right  programming  paradigm  for  each  division  of 
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the  program.  Moreover,  the  resulting  code  is  at  least  an  order  of  magnitude  shorter  than  the  corresponding 
Pascal  code  for  same  reason.  Consequently,  this  approach  significantly  reduces  development  time. 

The  remainder  of  this  report  first  presents  definitions  used  in  th's  n  or  « id  a  discussion  of  the 
mathematical-model  used  to  simulate  terrain  and  the  ASV  vehicle.  This  ioi’owed  by  a  description  of  the 
ditch  crossing  motion  coordination  rule-set,  and  the  use  of  Prolog  to  realize  both  ditch  crossing  motion 
coordination  and  normal  motion  coordination.  The  report  concludes  with  a  discussion  of  the  results  of  the 
investigation  and  suggestions  for  future  research. 


2.  Definitions 

In  this  report,  a  number  of  definitions  are  used  as  follows: 

Definition  1:  A  foothold  is  a  point  on  a  segment  of  terrain,  and  can  be  assigned  to  a  leg  while  the 
leg  is  in  the  air.  When  the  foot  of  a  leg  is  placed  on  the  terrain,  its  assigned  foothold  becomes  the  support 
point  of  the  leg.  A  foothold  associated  with  a  leg  can  be  changed  to  a  new  one  before  the  foothold  becomes 
a  support  point  [10]. 

Definition  2:  The  support  pattern  associated  with  a  given  set  of  leg  support  points  is  the  convex 
hull  of  the  vertical  projections  of  all  support  points  into  a  hoi  ontai  plane  [13]. 

Definition  3:  The  magnitude  of  the  stability  margin  at  time  t  for  an  arbitrary  support  pattern  is 
equal  to  the  shortest  distance  from  the  vertical  projection  of  the  vehicle  center  of  gravity  to  any  point  on  the 
boundary  of  the  support  pattern.  If  the  pattern  is  statically  stable,  the  stability  margin  is  positive. 
Otherwise,  it  is  not  defined  [13]. 
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Definition  4:  A  working  volume  is  associated  with  each  leg.  This  volume  is  a  subset  of  three- 
dimensional  space  defined  relative  to  the  body  and  consists  of  the  collection  of  points  which  can  be  reached 
by  the  foot  of  the  given  leg  [1 1,19]. 

Definition  5:  A  temporal  kinematic  margin  is  associated  with  each  foothold.  At  any  instant,  this 
margin  is  the  time  remaining  until  the  associated  leg  would  reach  the  boundary  of  its  working  volume  if  the 
foothold  were  used  as  a  support  point  [IS, 19]. 


3.  Vehicle  and  Terrain  Model 

While  the  vehicle  model  used  in  this  study  is  based  on  the  ASV,  it  represents  only  the  major 
vehicle  dimensions  und  components.  Specifically,  the  cabin  and  the  terrain  scanner  are  omiued  from  the 
simulation  model,  while  the  geometries  of  the  body  and  the  legs  are  identical  to  those  of  the  ASV. 
Therefore,  the  simulation  model  is  represented  by  a  simple  six-faced  box  with  each  leg  drawn  as  two  line 
segments  as  shown  in  Figure  1.  The  exact  vehicle  dimensional  data  can  be  found  in  other  literature  [2,7]. 
Differing  from  the  most  previous  simulation  studies  related  to  gaits  and  control  of  stepping 
[10,1 1,15,16,19],  which  simply  ignore  the  overlapped  portions  of  adjacent  working  volumes,  in  this  study, 
the  overlapped  portions  are  taken  into  account  during  the  foothold  selection  process  in  order  to  utilize  the 
full  kinematic  capability  of  the  vehicle. 

The  terrain  adopted  for  this  study  is  made  up  of  terrain  cells,  and  these  individual  cells  are  classified 
into  two  types  of  cells.  One  type,  called  a  permitted  ceil,  is  able  to  support  the  body  load  when  a  leg  steps 
on  it.  The  other  type,  named  a  forbidden  cell,  is  not  usable  because  of  unfavorable  terrain  conditions. 
Though  this  classification  is  complete  with  respect  to  individual  terrain  cells,  there  is  a  chance  that  a  group 
of  the  forbidden  cells  can  constitute  a  large  structured  obstacle  instead  of  being  randomly  distributed.  In  this 
study,  one  type  of  structured  large  obstacle,  a  ditch ,  is  considered  because  of  its  special  shape,  a  long  length 
and  a  relatively  narrow  width.  Due  to  its  shape,  the  most  effective  way  to  overcome  a  ditch  is,  if  possible, 
crossing  it  instead  of  avoiding  it  by  going  around  it.  However,  avoidance  may  be  a  better  choice  for  other 
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types  of  large  obstacles.  This  possibility  is  not  studied  in  this  report.  Rather,  the  simulation  terrain  is 
ternary  terrain  which  is  composed  of  permitted  cells,  forbidden  cells,  and  a  ditch .  A  typical  terrain  example 
utilized  in  this  study  is  shown  in  Figure  1.  A  celi  with  an  "X"  mark  is  a  forbidden  cell  or  a  part  of  a  ditch 
area  while  unmarked  cells  are  permitted.  A  ditch  is  shown  in  Use  middle  of  the  simulation  terrain. 

Forbidden  cells  on  this  terrain  can  be  designated  either  manually  by  an  operator  or  automatically  by  using  a 
random  number  generator  with  a  tiireshold  chosen  to  produce  a  specified  ratio  between  permitted  cells  and 
forbidden  cells  [11]. 

The  dimensions  of  each  cell  are  one  foot  by  one  foot.  This  size  is  comparable  to  that  of  the  feet  of 
the  ASV,  and  is  larger  than  the  resolution  of  the  terrain  scanner  [5,12], 

An  overall  block  diagram  of  the  program  developed  in  this  study  is  shown  in  Figure  2.  This  entire 
program  is  written  for  a  Symbolics  3650  Lisp  machine  [1 1,20,21].  Each  box  shown  is  an  object  that  is  an 
instance  of  a  Flavor  [18]  with  the  exception  of  the  Free  Qait  Coordinator  which  is  written  in  Symbolics 
Prolog  [22].  Like  the  physical  ASV  which  has  nine  major  pans,  namely,  a  body,  a  vision  sensor,  a  cab, 
and  six  legs,  the  simulation  object,  "ASV"  has  correspondingly  nine  component  objects,  "Body",  "Vision 
Sensor",  "Joystick",  md  "Lcgl"  through  "Leg6".  These  nine  objects  are  linked  to  "ASV"  through  a  part 
relation  [16,23],  Each  part  has  its  sub-parts,  and  again  is  linked  to  them  with  a  part  relation.  Differing 
from  the  nine  major  parts  which  have  visible  corresponding  parts  in  the  real  ASV,  the  subparts  of  the 
simulation  are  not  physically  tangible,  but  arc  introduced  because  of  their  functionalities  for  pmerram 
development.  For  example,  the  "Legl"  object,  which  is  a  part  of  the  "ASV"  object,  has  six  subparts: 

"Legl  Plan  Machine",  "Legl  Control  Machine",  "Legl  Executor",  "Lcgl  Contact  Sensor",  "Legl  Foothold 
Finder",  and  "Legl  TKM  Calculator".  Through  the  use  of  a  make-instance  function  in  an  appropriate 
Flavor  slot,  the  "Legl"  object  binds  all  of  these  subpsrts  into  one  group  with  the  part  relauon  [18].  In 
order  to  show  the  above  relations  among  the  objects  in  Figure  2,  the  six  subpart  objects  are  drawn  under  the 
"Legl"  object 

Besides  the  part  relation,  Figure  2  also  shows  the  hierarchical  control  structure  linking  the 
simulation  objects.  Specifically,  communication  is  restricted  between  objects  in  two  adjacent  levels  by  an 
assumption  that  upper  levels  have  the  right  to  access  status  information  at  lower  levels,  but  the  latter  must 
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receive  explicit  commands  from  upper  levels  to  update  their  internal  states.  For  example,  when  "  ASV\  the 
vehicle  object,  needs  "Legl"  to  support  its  body,  it  sends  a  "Place"  decision  to  "Legl"  and  continuously 
monitont  "Legl"  as  to  whether  "Legl"  has  begun  to  support  the  body  or  is  in  motion  to  try  to  reach  a 
foothold.  On  receiving  a  "Place"  decision  from  "ASV",  "Leg!"  sends  the  "Place”  decisions  to  "Legl  Plan 
Machine"  while  making  observations  of  this  machine.  This  type  of  message  passing  to  and  status 
observation  from  subordinates  continues  until  the  "Place"  decision  is  accomplished.  That  is,  when  the  foot 
of  "Legl"  actually  hits  the  ground,  the  contact  sensor  of  "Legl"  detects  the  event  and  changes  its  internal 
state.  The  state  change  of  "Legl  Contact  Sensor"  is  observed  by  "Legl  Executor"  and  by  "Legl  Control 
Machine".  In  this  way,  the  state  change  in  the  lowest  level  is  propagated  to  higher  levels  until  the  touch 
down  event  arrives  at  "ASV".  The  detailed  description  of  this  control  scheme  can  be  found  in  other 
literature  [16], 

The  joystick  object  simulates  the  physical  three-axis  joystick  of  the  ASV  through  the  use  of  six 
keys  on  the  simulation  computer  keyboard  to  Increment  or  decrement  each  of  the  three  rates  controlled  by 
the  joystick.  These  rates  are  forward  velocity,  lateral  velocity,  and  turn  rate,  all  in  body  coordinates.  The 
altitude  of  the  vehicle  above  the  terrain  and  its  orientation  in  roll  and  pitch  relative  to  the  terrain  are 
automatically  regulated  using  the  algorithms  described  in  [24]. 

While  an  elementary  representation  of  the  vision  sensor  is  included  in  the  program,  as  described  in 
the  above  discussion  of  terrain,  it  is  assumed  that  all  forbidden  cells  and  ditches  have  already  been  identified 
by  prior  terrain  analysis.  Of  course  this  assumption  does  not  represent  a  physical  limitation  of  the  ASV, 
but  as  made  merely  to  allow  this  simulation  to  be  focused  on  vehicle  control,  rather  than  on  vision. 

In  addition  to  simplification  of  vision,  this  simulation  also  ignores  leg  mass  in  order  to  avoid  the 
complexity  of  computing  a  center  of  gravity  which  moves  with  respect  to  the  body.  Moreover,  till  inertial 
forces  are  omitted  from  the  simulation.  That  is,  as  in  most  previous  simulation  studies  relating  to  gaits 
and  control  of  stepping  [8,9,10,1 1,16,19,25,26],  only  static  stability  is  considered  in  this  study.  While 
this  simplification  would  be  serious  in  high  speed  locomotion,  free  gaits  arc  most  appropriate  to  low  speed 
traversal  of  extremely  difficult  terrain,  so  the  authors  do  not  feel  that  this  is  a  serious  limitation  on  the 
applicability  of  the  results  of  this  investigation. 
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4.  Ditch  Crossing  Motion  Coordination 

When  the  vision  system  detects  the  existence  of  a  ditch,  the  vehicle  operation  mode  is  switched 
from  the  normal  free  gait  mode  [1 1 ,16]  to  the  ditch  crossing  mode.  In  contrast  to  the  normal  free  gait  mode 
which  performs  on-line  optimization  of  leg  stepping  under  an  environment  with  randomly  distributed  small 
obstacles  [11,16],  the  coordinator  in  the  ditch  crossing  mode  controls  the  vehicle  with  a  predetermined 
motion  sequence  in  order  to  effectively  overcome  a  ditch;  i,e.,  a  structured  large  obstacle.  The  ditch 
crossing  mode  is  composed  of  two  phases,  the  preparation  phase  and  the  main  phase,  and  these  two  phases 
are  sequentially  executed.  The  preparation  phase  provides  a  transition  period  from  the  normal  free  gait  mode 
to  the  ditch  crossing  mode,  and  the  main  phase  performs  the  actual  ditch  crossing  action. 

4.1  Preparation  Phase 

The  preparation  phase  is  composed  of  a  sequence  of  nine  states,  and  these  nine  states  are  grouped 
into  two  cycles  which  arc  named  Cycle  1  and  Cycle  2.  The  first  cycle,  Cycle  1,  consists  of  six  states  and 
takes  care  of  a  transition  from  the  normal  mode  to  the  ditch  crossing  mode.  During  the  execution  of  the 
first  cycle,  the  body  attitude  is  modified  to  be  suitable  to  cross  a  ditch.  The  second  cycle,  Cycle  2.  consists 
of  three  states  and  causes  the  vehicle  to  have  the  correct  leg  configuration  for  the  ditch  crossing  operation  in 
the  main  phase.  Thus,  during  the  execution  of  Cycle  2,  the  body  is  not  moved  at  all.  The  graphical 
representation  of  the  preparation  phase  is  shown  in  Figure  3. 

The  six  states  in  Cycle  1  are  Place  Legs  in  the  Air,  Back  Middle  Legs ,  Forward  Rear  Legs, 
Forward  Middle  Legs,  Forward  Front  Legs,  and  Lift  Middle  Legs  and  Move.  Body  movement  is  involved 
only  in  the  last  state.  The  first  state,  Place  Legs  in  the  Air,  represents  a  simple  action;  i.e.,  leg  placing, 
but  the  rest  of  the  states  represent  at  least  two  sequential  actions.  For  example,  during  the  Forward  Middle 
Legs  state,  the  middle  legs  are  lifted  from  the  ground  and  placed  on  the  ground  using  one  of  the  closest 
footholds  to  the  front  end  of  the  working  volumes  of  the  middle  legs.  Similarly,  in  the  Back  Middle  Legs 
state,  the  middle  legs  are  lifted  and  placed  at  one  of  the  closest  footholds  to  tin  back  end  of  the  working 
volumes  of  the  middle  legs. 
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The  first  cycle  of  the  preparation  phase  begins  with  the  Place  Legs  in  the  Air  state.  During  this 
state,  all  the  legs  in  the  air  are  placed  on  the  ground  without  any  body  movement.  At  most  three  legs  will 
be  placed  in  this  state  because  at  least  three  legs  must  support  the  body  at  all  times  to  maintain  the  stability 
of  the  vehicle.  When  a  leg  is  placed  in  this  state,  one  of  the  closest  footholds  to  the  front  end  of  the 
working  volume  of  each  leg  is  selected  as  a  stepping  position  on  the  ground.  Thus,  the  newly  placed  legs 
have  larger  temporal  kinematic  margins  (TKMs)  than  the  legs  already  on  the  ground. 

At  the  end  of  the  first  state,  all  six  legs  are  on  the  ground.  Thus,  the  middle  legs  can  be  used  for 
any  purpose  because  the  front  and  the  rear  four  legs  are  sufficient  to  make  the  vehicle  stable  as  long  as  these 
four  legs  are  within  their  kinematic  limits.  Therefore,  the  middle  legs  are  used  to  provide  maximum  TKMs 
for  the  front  and  the  rear  legs  in  the  following  four  states. 

The  Back  Middle  Legs  state  is  the  first  state  of  the  sequence  to  maximize  TKMs  of  the  front  and 
the  rear  legs.  The  middle  legs  are  lifted  and  placed  at  the  back  ends  of  their  working  volumes.  At  the 
completion  of  this  state,  the  middle  legs  arc  placed  behind  the  center  of  the  gravity  of  the  vehicle.  Thus, 
the  vehicle  can  maintain  its  stability  with  the  front  and  the  middle  legs  alone,  and  the  rear  legs  can  be  lifted. 

In  the  Forward  Rear  Legs  state,  the  rear  legs  are  lifted  and  placed  at  the  front  end  of  the  working 
volume  of  the  rear  legs.  Thus,  both  rear  legs  will  have  maximum  TKMs  at  the  end  of  this  state.  Though 
the  rear  leg  lifting  actions  are  inherently  safe,  before  lifting  one  of  the  rear  legs  the  vehicle  stability  is 
checked  to  ensure  that  the  vehicle  is  stable  without  the  rear  leg.  If  the  vehicle  is  not  stable,  the  leg  is  not 
lifted  from  the  ground,  and  the  ditch  crossing  operation  will  be  halted.  If  it  is  stable  without  the  leg,  the 
leg  is  lifted.  After  the  first  rear  leg  is  lifted  safely,  the  same  test  is  performed  on  the  other  rear  leg  before  it 
is  lifted.  This  type  of  test  is  always  performed  before  lifting  any  leg  from  the  ground  during  the  ditch 
crossing  operation  in  order  to  ensure  safe  operation,  At  the  end  of  the  Forward  Rear  Legs  state,  both  of  the 
rear  legs  arc  placed.  Again,  the  middle  legs  become  redundant  for  the  vehicle  stability. 

In  the  Forward  Middle  Legs  state,  the  middle  legs  are  lifted  and  placed  at  the  front  end  of  the 
working  volumes  of  the  middle  legs.  Because  the  new  support  points  of  the  middle  legs  arc  ahead  of  the 
center  of  the  gravity  of  the  vehicle,  the  vehicle  now  can  be  safely  supported  by  the  middle  and  the  rear  legs. 
Thus,  the  front  legs  can  be  lifted  from  the  ground  without  harming  the  stability  of  the  vehicle. 
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In  the  Forward  Front  Legs  state,  the  front  legs  are  lifted  and  placed  at  the  edge  of  the  vehicle  side  of 
the  ditch.  This  has  to  be  done  because  the  new  support  points  of  the  front  legs  will  be  the  last  ones  on  the 
vehicle  side  of  the  ditch.  This  requirement  is  not  hard  to  meet  since,  as  long  as  the  edge  of  the  ditch  is 
included  in  the  working  volume  of  the  front  legs,  the  front  legs  can  always  be  put  in  the  right  position. 
However,  the  opposite  side  of  the  edge  of  the  ditch  should  not  be  included  in  the  working  volume  in  order 
to  prevent  its  being  used  as  possible  footholds  for  the  front  legs.  Therefore,  there  is  a  range  of  the  vehicle 
locations  with  respect  to  the  near  edge  of  the  ditch  so  that  the  vehicle  can  select  the  right  stepping  positions 
for  the  front  legs.  This  range  is  shown  in  the  following  equation: 

P  +  yL-DW<  DCIR  <  P  +  yL  .  (l) 

where  DCIR  :  Ditch  Crossing  Initiation  Range 

with  respect  to  gravity  center  of  the  vehicle 
P  :  Pitch  between  adjacent  legs 

L  :  Longitudinal  length  of 

working  volume  for  each  legs 
DW  :  Ditch  Width. 

To  understand  this  relationship,  it  should  be  recognized  that  the  distance  from  the  vehicle's  center  to  the 
front  ends  of  the  working  volumes  of  the  front  legs  is  the  sum  of  the  pitch  between  the  middle  and  the  front 
legs  and  half  of  the  longitudinal  length  of  the  working  volume  of  the  front  legs.  Thus,  the  meaning  of 
Eq.  (1)  is  that  when  the  ditch  crossing  operation  is  initiated,  the  front  ends  of  the  working  volumes  of  the 
front  legs  should  be  positioned  between  the  near  edge  and  the  far  edge  of  the  0  ich.  Evidently,  if  ditch 
crossing  is  initiated  anywhere  in  the  above  range,  the  ditch  crossing  opemtion  will  not  be  hampered  since 
the  body  attitude  and  the  leg  stepping  positions  arc  corrected  during  the  preparation  phase.  It  should  be  also 
noted,  however,  that  if  DW  is  less  than  3  ft,  the  vision  system  does  not  have  to  detect  the  existence  of  the 
ditch  at  all.  The  normal  plan  developed  in  [16]  is  capable  of  handling  such  ditch  width  without  any 
problem. 

At  the  completion  of  the  Forward  Front  Legs  slate,  the  front  tegs  will  be  placed  on  the  vehicle 
side  edge  of  the  ditch.  Again,  the  middle  legs  become  redundant. 
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In  the  Lift  Middle  Legs  and  Move  state,  the  middle  legs  are  lifted  and  the  body  is  moved  into  the 
ditch  area  until  at  least  one  of  the  supporting  legs  (the  front  and  the  rear  legs)  reaches  its  kinematic  limit.  If 
too  many  obstacles  have  not  interfered  with  the  operations  of  the  previous  four  states,  there  will  be  a  high 
probability  for  the  front  legs  to  reach  their  kinematic  limits  first  because  footholds  in  the  front  most 
portion  of  the  working  volumes  of  the  front  legs  may  be  excluded  by  the  location  of  the  ditch.  Thus,  at  the 
end  of  this  state,  the  vehicle  body  is  fully  pushed  into  the  ditch  area  under  the  constraints  of  the  current  leg 
configuration,  and  its  movement  is  stopped.  At  this  point,  although  the  body  position  and  the  front  leg 
positions  are  right  for  the  ditch  crossing,  the  rear  leg  positions  are  not  appropriate  because  they  are  already 
near  their  kinematic  limits.  Thus,  further  body  movement  is  very  limited  or  impossible  depending  on  the 
kinematic  margins  of  the  rear  legs.  Even  though  the  opposite  side  of  the  edge  may  not  be  reachable  by  the 
front  legs  at  the  end  of  the  Lift  Middle  Legs  and  Move  state,  the  vehicle  can  cross  the  ditch  if  it  moves 
further  into  the  ditch  area  by  eliminating  the  kinematic  problem  of  the  rear  legs  and  if  the  ditch  width  is 
narrower  than  the  vehicle  ditch  crossing  capability.  In  the  second  cycle  of  the  {reparation  phase,  the 
kinematic  limits  of  the  rear  legs  are  eliminated. 

The  second  cycle  of  the  preparation  phase  has  three  states  and  starts  with  the  Back  Middle  Legs 
state.  No  body  movement  is  involved  in  the  second  cycle,  but  the  stepping  positions  of  the  rear  and  the 
middle  legs  are  rearranged. 

In  the  first  stale,  the  middle  legs,  which  are  redundant  to  make  the  vehicle  stable,  arc  lifted  and 
placed  near  the  back  end  of  their  working  volumes.  Because  the  new  support  points  of  the  middle  legs  arc 
behind  the  center  of  the  gravity  of  the  vehicle,  the  rear  legs  can  be  lifted  without  harming  the  vehicle 
stability. 

In  the  Forward  Rear  Legs  state,  the  rear  legs  arc  lifted  and  placed  near  the  front  end  of  their  working 
volumes.  Thus,  the  rear  legs  obtain  maximum  kinematic  margins.  At  this  point,  though  the  rear  legs 
provide  maximum  body  movement  potential,  the  middle  legs  prohibit  further  body  movement. 

In  the  Forward  Middle  Legs  stale,  the  middle  leg  kinematic  problem  is  eliminated.  The  middle 
legs  are  lifted  and  placed  at  the  front  end  of  the  working  volume  of  the  middle  legs.  Therefore,  both  the 
middle  and  the  rear  legs  have  their  maximum  kinematic  margins,  while  the  body  is  completely  pushed  into 
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the  ditch  area.  In  contrast  to  the  middle  and  the  rear  legs,  the  front  legs  should  be  at  their  kinematic  limits 
because  the  front  legs  stepping  positions  have  not  changed  since  they  were  at  their  kinematic  limits  at  the 
end  of  Cycle  1.  If  the  front  legs  are  now  lifted,  vehicle  body  movement  can  be  resumed.  This  will  be  the 
firat  action  of  the  following  phase,  the  Main  Phase. 

In  summary,  at  the  end  of  the  preparation  phase,  the  body  is  fully  moved  into  the  ditch  area  within 
the  limits  of  the  stability  of  the  vehicle  and  die  kinemadcs  of  the  legs.  The  middle  legs  and  the  rear  legs 
are  fully  forward  to  enhance  the  ditch  crossing  capability,  and  the  front  legs  are  ready  to  be  lifted  from  the 
ground.  This  preparation  allows  the  vehicle  to  cross  a  wider  ditch  than  the  longitudinal  length  of  the 
vehicle  legs'  working  volumes.  Though  the  other  side  of  the  ditch  is  not  included  in  the  front  legs'  working 
volumes  at  the  end  of  the  preparation  phase,  the  body  can  move  forward  as  long  as  the  vehicle's  stability  is 
maintained  and  the  leg  kinematic  limits  are  not  reached.  That  is,  if  the  other  side  of  the  ditch  is  included  in 
the  front  leg's  working  volume  before  the  vehicle  becomes  unstable  and  before  the  other  legs  reach  their 
kinematic  limits,  the  other  side  is  reachable  by  the  front  legs.  If  the  front  legs  can  be  placed  on  the  other 
side  within  the  vehicle's  kinematic  and  stability  limitations,  then  the  vehicle  can  cross  the  ditch  because  the 
ASV  legs'  working  volumes  are  identical  and  because  the  pitches  between  the  front  legs  and  the  middle  legs 
and  between  the  middle  legs  and  the  rear  legs  are  the  same. 

As  a  result  of  the  above  arguments,  the  maximum  ditch  width  can  be  crossed  by  the  ASV,  which 
has  identical  working  volumes  for  all  legs  and  the  equal  pitches  between  the  front  and  the  middle  legs  and 
between  the  middle  and  the  rear  legs,  is  determined  by  both  the  pitch  length  and  the  length  of  the 
longitudinal  working  volumes  of  the  legs.  Specifically,  (he  maximum  ditch  width  can  be  crossed  by  the 
ASV  is  given  by: 

MDW  -  P  +  L  -  SM  -  SDE  .  (2) 

where  MDW  :  Maximum  Ditch  Width 

P  :  Pitch  between  adjacent  legs 

L  :  Longitudinal  length  of 

working  volume 
SM  :  Safety  Margin 

SDE  :  Search  Digitization  Effect. 
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As  can  be  seen,  the  maximum  ditch  width  (MDW)  is  calculated  by  adding  the  pitch  between  adjacent  legs 
and  the  half  of  the  longitudinal  length  of  the  working  volumes  of  the  legs,  and  then  by  subtracting  the 
safety  margin  (SM)  and  the  search  digitization  effect  (SDE).  Thu  safety  margin  is  a  prescribed  margin 
ensuring  safe  operation  of  the  vehicle.  The  search  digitisation  effect  is  an  artifact  of  the  foothold  search 
process  resulting  from  a  one  foot  by  one  foot  grid  search,  With  the  dimensions  of  the  simulation  model 
discussed  in  [2,7],  the  MDW  becomes  8.S  ft  when  the  SM  and  the  SDE  are  0.S  ft,  respectively. 

4.2  Main  Phase 

The  Main  Phase  is  composed  of  three  cycles  The  first  and  the  third  cycles  are  composed  of  three 
states  each,  while  the  second  cycle  contains  only  one  state.  The  first  cycle  in  the  main  phase  in  the 
program  is  named  Cycle  3  to  show  continuation  from  the  preparation  cycles.  Consequently,  Cycle  1  and 
Cycle  2  belong  to  the  Preparation  Phase ,  and  Cycle  3  through  Cycle  5  belong  to  the  Main  Phase .  A 
graphical  representation  of  the  Main  Phase  is  shown  in  Figure  4. 

The  first  cycle,  Cycle  3  is  composed  of  three  states,  Move  Forward  Front  Legs,  Move  Back  Middle 
Legs,  and  Move  Forward  Rear  Legs.  During  this  cycle,  the  front  legs  cross  the  ditch,  and  the  rear  legs  are 
prepared  to  replace  the  middle  legs  which  will  cross  the  ditch  in  the  following  cycle.  In  this  cycle,  the 
vehicle  body  is  allowed  to  move  forward  whenever  possible.  Therefore,  all  the  state  names  are  prc-fixed 
with  "Move",  and  each  state  is  composed  of  three  actions,  leg  lifting,  body  movement,  and  leg  placement. 

In  the  Move  Forward  Front  Legs  state,  first,  the  front  legs  are  lifted  while  the  body  is  not  moved. 
As  soon  as  both  front  legs  are  lifted  from  the  ground,  the  second  action  is  performed,  which  is  a  forward 
body  movement.  This  body  movement  is  sustained  until  the  middle  legs  limit  this  movement  because  the 
middle  leg  positions  with  respect  to  the  center  of  the  vehicle  gravity  determine  the  stability  margin  when 
only  the  middle  and  the  rear  legs  support  the  body.  Though  the  middle  legs  can  kinematically  move  behind 
the  center  of  gravity,  they  should  be  stopped  in  front  of  the  center  of  gravity  to  maintain  the  safety  stability 
margin. 

When  the  body  movement  is  stopped  with  the  completion  of  the  second  action  of  the  current  state, 
the  opposite  side  of  (lie  ditch  will  be  included  in  the  working  volumes  of  the  front  legs  if  the  width  of  the 
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ditch  is  narrower  than  MDW.  Thus,  the  third  action  of  the  Move  Forward  Front  Legs  state  follows,  in 
which  the  front  legs  are  placed  on  the  opposite  side  of  the  ditch.  Thus,  the  middle  legs  become  redundant 
for  the  vehicle  stability. 

In  the  second  state  of  Cycle  3,  the  Move  Back  Middle  Legs  state,  the  middle  legs  are  lifted  from 
the  ground.  The  vehicle  body  movement  is  resumed  because  the  movement  is  restricted  by  the  middle  legs 
to  maintain  the  vehicle  stability  margin.  The  body  movement  is  continued  until  any  one  of  the  supporting 
legs  meets  its  kinematic  limit.  Specifically,  the  body  will  be  moved  until  one  or  both  rear  legs  reach  its  or 
their  kinematic  limits  because  the  kinematic  margins  of  the  rear  legs  have  been  used  to  move  the  body  in 
the  previous  state,  but  those  of  the  front  legs  have  been  just  maximized  in  the  previous  cute,  the  Move 
Forward  Front  Legs  state  of  Cycle  3.  This  effect  can  be  easily  seen  in  the  second  drawing  of  Cycle  3 .-State 
2  in  Figure  4,  and  "Rear  Legs"  written  on  the  top  of  the  second  drawing  shows  the  termination  condition  of 
the  current  body  movement.  When  the  kinematic  limits  of  the  rear  legs  stop  the  body  movement,  the  third 
action  of  the  current  state  is  performed,  which  is  to  place  the  middle  legs  at  the  back  end  of  their  working 
volumes.  Because  the  middle  legs  are  placed  behind  the  center  of  gravity,  the  rear  legs  can  be  lifted  from  the 
ground  while  the  front  and  the  middle  legs  stably  support  the  body. 

In  the  Move  Forward  Bear  Legs  state,  which  is  the  third  state  of  CyclcS,  first,  the  rear  legs  are 
lifted  from  the  ground.  As  soon  as  the  rear  legs  are  lifted,  the  body  movement  is  resumed.  However,  the 
body  movement  is  immediately  blocked  by  the  middle  legs  which  have  placed  back  in  the  previous  state. 
Thus,  the  third  action  of  the  current  state,  which  places  the  rear  legs  at  the  front  end  of  their  working 
volumes,  is  immediately  started.  Consequently,  very  little  body  movement  is  involved  in  this  state,  but 
the  rear  legs  gain  large  TKMs  so  that  the  body  can  be  moved  further  in  the  next  cycle.  Therefore,  though 
the  other  side  of  the  ditch  may  not  reachable  by  the  middle  legs  under  the  current  body  position,  the  new 
body  movement,  which  will  be  performed  in  the  next  cycle,  will  make  this  possible. 

The  second  cycle  of  the  main  phase,  Cycle  4,  is  composed  of  one  state,  the  Move  Forward  Middle 
Legs  state.  In  this  cycle,  the  middle  legs  will  be  moved  to  the  other  side  of  the  ditch.  The  first  action  is 
lifting  the  middle  legs  so  that  the  body  movement  can  be  resumed.  This  movement  will  last  as  long  as  the 
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front  legs  have  positive  TKMs  because  the  TKMs  of  the  front  legs  have  already  been  partially  consumed  in 
the  previous  cycle.  This  is  shown  in  the  second  drawing  of  Cycle  4:State  I  in  Figure  4. 

When  the  body  movement  is  stopped,  the  other  side  of  the  ditch  is  reachable  by  the  middle  legs 
because  the  geometries  of  the  front  and  the  middle  legs  are  identical  and  because  the  working  volumes  of  the 
front  and  the  middle  legs  overlap  slightly  at  the  rear  end  of  the  former  volume  and  the  front  end  of  the  latter 
volume.  As  soon  as  the  middle  legs  are  positioned  on  the  other  side  of  the  ditch,  this  cycle  is  terminated. 

The  last  cycle,  Cycle  5,  which  is  the  third  cycle  of  the  main  phase,  takes  care  of  the  ditch  crossing 
action  of  the  rear  legs.  This  cycle  is  composed  of  the  three  states,  the  Move  Forward  From  Legs  state,  the 
Move  Back  Middle  Legs  state,  and  the  Move  Forward  Rear  Legs  state.  These  three  states  are  the  same  that 
of  Cycle  3.  This  is  not  a  coincidence,  but  an  expected  consequence  of  the  geometrical  symmetry  of  the 
front  and  the  rear  legs. 

In  the  Move  Forward  Front  Legs  states,  the  kinematic  problems  of  the  front  legs  which  block 
further  body  movement  are  relieved  because  the  first  action  of  this  state  is  to  lift  the  front  legs  from  the 
ground.  Thus,  the  body  movement,  which  is  the  second  action  of  this  state,  is  resumed,  and  is  terminated 
by  the  positions  of  the  middle  legs  with  respect  to  the  body  because  the  positions  of  the  middle  legs 
determine  the  stability  of  the  vehicle.  When  this  state  is  terminated,  the  front  legs  are  placed  on  the  ground. 

In  the  Move  Back  Middle  Legs  state,  the  body  movement  is  resumed  as  soon  as  the  middle  legs  are 
lifted  from  the  ground.  This  movement  will  last  until  the  kinematic  limits  of  the  rear  legs  arc  reached. 
When  this  condition  is  met,  die  body  movement  is  stopped  and  the  middle  legs  are  placed  as  far  backward  as 
possible  so  that  the  middle  legs  can  support  the  body  together  with  the  front  legs.  Consequently,  the 
middle  legs  will  be  placed  at  the  edge  of  the  ditch  because  this  edge  is  in  the  working  volumes  of  the  middle 
legs. 

The  Move  Forward  Rear  Legs  state,  which  is  the  last  stale  in  the  last  cycle,  causes  the  rear  legs  to 
cross  the  ditch.  The  first  action  is  to  lift  the  rear  legs  from  the  ground.  Body  movement  is  then  resumed 
and  continued  until  the  other  side  of  the  ditch  is  reachable  by  the  rear  legs.  Again,  this  will  be 
accomplished  because  the  working  volumes  of  the  middle  and  the  rear  legs  slightly  overlap.  Finally,  all  the 
legs  are  across  the  ditch.  Thus,  the  ditch  crossing  operation  has  accomplished  and  the  operational  mode  is 
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switched  back  to  the  normal  mode,  and  the  Free  Gait  Motion  Coordinator  [16]  regains  control  of  the, 
vehicle. 


5.  Program  Implementation 

The  top  level  Ternary  Terrain  Motion  Coordinator  is  written  in  Symbolics  Prolog  because  of  its 
easy  translation  characteristics  from  natural  language  to  a  computer  program,  and  because  of  its 
straightforward  interface  to  Symbolics  Lisp  language  in  which  the  rest  of  the  program  is  written.  The 
Prolog  program  is  listed  in  Figure  5,  It  is  composed  of  three  functional  groups  of  predates.  The  first 
group  controls  the  flow  of  the  whole  program,  while  the  second  does  logic  processing  which  generates 
commands  for  the  vehicle  body  and  legs.  The  last  group  is  responsible  for  bridging  between  the  program 
written  in  Prolog  and  the  robot  program  in  Flavor  objects.  This  is  accomplished  through  the  Lisp  function 
call  facility  provided  by  Symbolics  Prolog,  Specifically,  anything  following  the  "is"  predicate  in  a  Prolog 
clause  may  be  either  a  Prolog  arithmetic  function  or  the  name  of  a  Lisp  function  [22].  If  a  Lisp  function 
name  follows  the  "is"  predicate,  it  is  evaluated  according  to  its  definition  inside  the  Lisp  environment.  In 
the  program  of  Figure  S,  arguments  following  "is"  predicates  are  names  of  Lisp  functions,  and  make 
connections  to  the  Lisp  environment.  A  returned  value  resulting  from  a  Lisp  function  call  may  be  used  to 
instantiate  a  variable  preceding  the  "is"  Prolog  predicate  or  test  whether  the  returned  value  matches  a  value 
preceding  the  "is"  predicate.  In  the  former  case,  the  subgoal  "is"  always  succeeds,  but  the  latter  case,  only 
when  two  values  agree  does  the  "is"  subgoal  succeed.  In  the  program,  only  the  former  case  is  used.  The 
Lisp  portion  of  the  program  is  listed  in  the  appendix  attached  at  the  back  of  this  report. 

The  Prolog  program  is  started  by  typing  "robot"  on  the  computer  console.  The  robot  clause  is  the 
first  line  of  the  program.  After  the  initialization  process  is  done,  it  makes  the  loop  clause  repeat  Thus,  it 
determines  the  flow  of  the  whole  program. 

The  loop  c'ause  is  composed  of  three  subgoals,  getjommand,  plan,  and  execute.  This  shows  the 
flow  of  the  program  execution  for  each  loop.  Based  on  the  input  command  from  the  joystick,  motion  is 
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planned,  and  the  planned  motion  Is  executed.  Then,  the  executed  motion  is  drawn  on  the  screen  by  the 
drawjobot  clause  which  actually  calls  a  corresponding  Lisp  function,  graphical jiisplay, 

The  plan  subgoal  of  the  loop  clause  has  two  alternatives,  frce_galts_motion_coordination _plan  and 
ditch  jrosslngjnotionjcoordlnation _plan ,  In  the  Prolog  program,  the  ditch  jnode  subgoal  is  tested  first 
because  ditch ^crossing  jnotionjoordination  jtlan  deals  with  a  more  specific  case  than  the  other,  If  the 
ditch  jnode  subgoal  succeeds,  then  ditch crossing jnotionjcoordlnadon j>lan  is  executed.  If  not,  then 
free jgaitsjnotionjoordination j>lan  is  executed.  Thtfree jgaits_motion_coordination jilan  clause  is 
composed  of  update jobotjtate,  checkjkmjimit,  leg  jtlan ,  body jtlan ,  and  generate  ^decision  subgoals. 
The  first  and  the  second  subgoals  update  the  state  and  the  body  position  of  the  vehicle  and  check  kinematic 
problems  of  the  legs.  The  third  subgoal,  leg_plan,  performs  on-line  optimization  for  leg  coordination 
using  the  free  gait  strategy  [11,16],  Based  on  the  leg  plan,  the  fourth  subgoal,  body jtlan,  plans  the  body 
movement  to  enhance  the  vehicle  stability.  Finally,  the  generate ^decision,  subgoal  sends  decisions  to  the 
"AS  V"  robot  flavor  object.  A  detailed  description  of  thence _galtsjnotion_coordination  jtlan  can  be  found 
in  other  literature  [11,16], 

The  ditch_crossingjnotion_coordination  jlan  and  the  related  clauses  implement  the  ditch  crossing 
coordination  discussed  In  the  previous  section.  There  are  two  dltch_crossingjnotion_coordlnation  jtlan 
clauses  In  the  program,  and  the  first  clause  checks  the  termination  condition  while  the  second  clause 
performs  the  ditch  crossing  planning.  If  the  ditch  crossing  activity  is  not  completed,  then  the  first  clause 
fails,  and  the  second  ditchjrossingjnoticnjoordination  jtlan  clause  is  executed.  Thus,  the  cycle jtlanner 
clause  is  called  into  an  action. 

The  cycle  jtlanner  clause  and  related  clauses  follow  the  above  two  ditch j>lan  clauses.  This  group 
of  clauses  is  named  "Cycle  Planner".  The  first  clause  of  the  "Cycle  Planner"  group,  ditch  j)lan_done, 
checks  the  completion  of  the  ditch  crossing  plan.  The  two  cycle  jjlanner  clauses  take  care  of  ditch  plan 
cycle  changes  from  cycle  1  to  cycle  5  by  increasing  the  cycle  number  whenever  one  cycle  is  completed. 
Therefore,  the  ditch  jtlan_done  clause  succeeds  as  soon  as  cycle  5  is  finished  because  the  cycle  number 
becomes  6  immediately  after  li  e  completion  of  Cycle  5.  The  last  subgoal  of  ditch  jtlan  jione,  is 
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idlejycle,  which  is  a  dummy  plan  without  any  leg  planning.  It  is  introduced  to  fill  the  gap  for  the 
transition  between  cycles. 

The  subgoals  used  in  the  cycle  jlanner  clauses,  onejyclejlone  and  planjycle,  are  grouped  in  the 
following  part  of  the  program,  which  is  called  the  "Plan  Cycle  Dispatcher"  group.  The  onejyclejlone 
clause  checks  the  termination  condition  of  one  plan  cycle,  and  the  five  planjycle  clauses  execute  the 
appropriate  ditch  plan  cycles  based  on  the  plan  cycle  number  which  is  given  through  the  planjycle  fact  in 
the  Prolog  data  base. 

The  ditch jhnjycle  clauses,  ditch  jlanjyclel  through  ditch  jlanjycleS,  form  another  group 
called  "Cycles"  in  the  program  following  the  "Plan  Cycle  Dispatcher"  group.  The  first  subgroup, 
ditch  jlanjyclel,  has  seven  clauses.  The  first  clause  of  this  subgroup  takes  care  of  the  initial  state 
transition,  and  the  rest  of  them  represent  the  six  states  in  cycle  1  discussed  in  the  previous  section. 
Specifically,  the  first  clause  retracts  planjtate(start),  which  is  a  cycle  starting  fact,  from  the  Prolog  data  base 
and  asserts  a  new  fact,  planjtate(place JegsJnjhejir),  which  is  the  name  of  the  first  state.  After  changing 
the  Prolog  data  base,  the  first  clause  executes  the  place  JegsJnjhejir  subgoal,  which  performs  the  place 
legs  in  the  air  state  in  Cycle  1.  When  the  place  JegsJnjhejir  subgoal  is  executed,  the  first  clause 
provides  the  next  state  information  for  the  subgoal  so  that  when  the  current  subgoal  is  completed  the  correct 
information  about  the  succeeding  state  is  asserted  in  the  data  base.  The  second  clause  through  the  sevenLh 
clause  sequentially  represent  six  states  in  cycle  1.  Thus,  these  ordered  clauses  represent  the  sequence  of  state 
transitions  among  the  six  states.  When  the  last  clause  calls  the  Hft_middleJegsjnd_move  subgoal, 
one  jlanjyclejlone  is  given  instead  of  the  name  of  the  next  state  to  assert  the  cycle  termination  fact  in  the 
data  base.  This  structure  is  repeated  for  the  rest  subgroups  of  the  "Cycles"  group,  ditch  jlanjyclel  through 
ditch jlanjycleS. 

The  following  group  called  "States"  is  composed  of  10  different  subgroups,  and  each  subgroup  is 
composed  of  two  clauses.  These  clauses  accept  Information  about  the  next  state  so  that  the  next  state 
information  is  asserted  when  the  current  state  is  completed.  However,  only  the  first  clause,  which  takes 
care  of  its  state  transition,  utilizes  the  next  state  information.  The  second  clause  ignores  the  next  state 
information  and  executes  a  subgoal  whose  name  is  its  clause  head  name  prc-flxed  with  "do_".  Additionally, 
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both  clauses  of  each  subgroup  determine  the  body  movement  by  executing  either  the  stop  or  the  move 
subgoal  depending  on  the  needs  of  each  state  described  in  the  previous  section. 

The  "State  Executors"  clause  group  follows  the  "States"  clause  group.  This  clause  group  is 
composed  of  11  subgroups  of  clauses.  Among  them,  10  subgroups  are  responsible  for  execution  of  10 
states  in  the  "States"  group,  while  the  eleventh  subgroup  takes  care  of  the  body  movement,  such  as  move, 
stop,  clear  jnovejnemory,  and  movejione,  Therefore,  except  for  the  last  subgroup,  each  subgroup  shows  a 
sequence  of  actions  within  a  state,  which  are  described  in  the  previous  section.  For  example,  the 
dojntckjnlddlejegs  clause  subgroup,  which  is  the  first  subgroup  of  the  "State  Executors"  group,  is  started 
with  three  major  clauses.  The  first  clause,  backjniddleJegsjione,  tests  the  state  termination  condition,  and 
the  second  and  the  third  clauses  perform  a  sequence  of  actions,  which  are  lifting  the  middle  legs  and  then 
placing  them  at  the  back  side  of  their  reachable  areas.  The  second  clause  tests  whether  both  middle  legs  are 
lifted  by  executing  the  subgoal,  alljniddlejegsjifted,  Initially,  this  test  should  fail.  Thus,  the  third  clause 
is  executed.  After  the  third  clause  is  executed  twice,  the  both  middle  legs  will  have  been  lifted  from  the 
ground  because  the  UftjniddleJegs  subgoal  causes  one  middle  leg  to  be  lifted  from  the  ground  at  a  time.  In 
the  do  backjnlddlejegs  subgroup,  there  arc  two  HftjniddleJegs  clauses.  The  first  clause  performs  the  leg 
lifting  action  by  selecting  one  middle  leg  and  then  causing  it  to  be  lifted  from  the  ground,  while  the  second 
clause  performs  a  default  action  by  always  succeeding.  Only  when  the  both  middle  legs  are  lifted  from  the 
ground,  is  the  middle  leg  placement  executed.  Specifically,  when  the  all  middle  JegsJif ted  subgoal  in  the 
second  clause  of  this  subgroup  is  satisfied,  the  place  jniddlejegsjback  subgoal  is  executed.  If  the  middle 
legs  are  placed  on  the  ground  again,  then  the  backjniddleJegsjione  clause,  which  is  the  first  clause  of  the 
current  subgroup,  succeeds  because  the  all  jniddlejegsjlfted  subgoal  has  been  satisfied  by  the 
middle  Jegs(Hfled)  fact  which  was  asserted  when  middle  legs  were  lifted  from  the  ground,  and  because  the 
allmiddlelegs j>laced  subgoal  is  now  satisfied.  Before  completing  the  backjniddleJegsjione  clause,  the 
first  clause  of  the  current  subgroup,  the  clear jniddieJiftedjnemory  and  the  clear  jnovejnemory  subgoals 
clear  residual  facts  generated  during  execution  of  the  dojbackjniddlejegs  clauses  in  order  not  to  interfere 
with  the  execution  of  the  following  "State  Executions"  subgroup  clauses. 
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Rest  of  the  subgroups  in  the  "State  Executors"  group  have  the  exactly  same  structure  that  of  the 
do_back_middleJegs  clause  subgroup.  Specifically,  one  state  termination  clause  is  followed  by  two  state 
execution  clauses  which  are  pre-fixed  with  "do_"  and  related  clauses  which  support  these  leading  three 
clauses.  If  the  related  clauses  are  already  available,  they  are  not  duplicated  by  adding  them  in  the  subgroup. 

The  only  exception  to  the  above  structure  is  the  fifth  subgroup,  the  dojiftjrdddlejegs  clause 
subgroup.  This  subgroup  is  composed  of  one  clause,  and  there  is  no  clause  to  test  the  state  termination 
condition.  The  time  required  to  complete  the  middle  leg  lifting  action  in  the  liftjn tddleJegsjmdjnove 
state,  which  is  the  only  state  that  utilizes  the  dojiftjrdddlejegs  clause,  is  considerably  shorter  than  that 
needed  to  complete  the  body  movement  in  the  state.  Thus,  the  leg  lifting  action  is  always  guaranteed  before 
the  current  state  is  terminated. 

The  last  group  of  clauses  is  named  "Plan  Libraries".  These  clauses  are  used  by  both 
ditch jrosslngjnotionjoordinatlon  jtlan  and  free _gaitsjnotion_coordination jtlan.  This  group  is 
composed  of  two  subgroups,  body  j>lan  and  generate ^decision.  The  latter  subgroup  sends  planned  leg 
motions  through  decisions  to  the  robot,  "ASV",  which  is  a  flavor  object.  It  sends  them  one  by  one  until 
all  the  decisions  in  the  Prolog  data  base  are  exhausted.  The  former  subgroup  takes  care  of  body  movement 
by  executing  speed  j>lan  and  trajectory jtlan.  The  speed _plan  clauses  control  the  speed  of  body  movement 
and  the  trctjectory  jtlan  clauses  modify  body  movement  trajectory  in  order  to  increase  the  stability  margin  of 
the  vehicle  using  a  "push"  operation  which  causes  the  gravity  center  of  the  vehicle  to  move  away  from  the 
boundary  of  the  current  supporting  pattern  [  16]. 


6.  Discussion 

Performance  tests  were  carried  out  for  various  terrain  conditions  by  making  the  ASV  follow  a 
prescribed  standard  trajectory.  The  standard  trajectory  is  a  straight  line  across  the  model  terrain  from  one 
side  to  the  other  side  while  crossing  a  ditch  oriented  perpendicular  to  the  direction  of  vehicle  motion.  No 
failures  to  complete  tlte  standard  trajectory  were  observed  for  any  terrain  containing  up  to  a  8  ft  width  ditch 
if  no  randomly  distributed  forbidden  cells  were  included  in  the  terrain.  However,  when  forbidden  cells  were 
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added  to  the  terrain  with  a  8  ft  width  ditch,  the  performance  was  severely  degraded,  If  the  randomly 
distributed  forbidden  cells  occupied  30  percent  or  more  of  the  area  of  the  non-ditch  portion  of  the  whole 
simulation  terrain,  the  ASV  always  failed  to  complete  the  standard  trajectory.  Specifically,  the  ditch 
crossing  operation  was  halted  because  the  random  obstacles  on  the  ground  prevented  the  ASV  firom  using 
the  most  favorable  stepping  positions  near  the  ditch.  However,  when  the  width  of  a  ditch  was  reduced  to 
7  ft,  no  failures  in  ditch  crossing  operations  were  observed.  Rather,  the  capability  to  overcome  randomly 
distributed  forbidden  cells  became  the  bottle  neck  which  determined  whether  the  ASV  could  complete  the 
standard  trajectory  or  not  Overall,  when  less  than  70  percent  of  the  total  terrain  cells  were  the  forbidden 
cells,  the  program  made  the  ASV  follow  the  standard  trajectory  without  great  difficulties. 

One  of  the  advantages  of  using  object-oriented  programming  for  the  ASV  object  and  its  subobjects 
was  the  easy  extension  to  a  new  ASV  with  additional  functionality  required  for  the  ditch  crossing 
maneuvers.  Specifically,  this  was  accomplished  by  using  the  inheritance  mechanism  provided  by  Flavors. 
The  original  ASV  was  an  instance  of  "robot"  class,  and  the  new  extended  ASV  is  an  instance  of  "ditch- 
robot"  class.  The  latter  class  is  defined  as  a  subclass  of  the  former  class.  Thus,  the  entire  functionality  of 
the  "robot"  class  became  available  to  the  "ditch-robot"  class  through  the  inheritance  mechanism.  The  newly 
required  capabilities  were  added  to  the  "ditch-robot”  class  using  "defmethod"  which  defines  the  functionality 
of  a  class  in  Flavors.  The  result  was  remarkable.  The  additional  code  wiitten  for  the  new  "ditch-robot" 
class  was  less  than  10%  of  the  size  of  the  original  "robot"  class,  and  roughly  more  than  95%  of  the  original 
code  was  reused. 

One  of  advantages  of  rule-based  control  of  motion  coordination  is  the  ease  of  extension  of 
coordination  logic  resulting  from  the  fact  that  Individual  rules  or  a  group  of  rules  define  an  independent 
piece  of  behavior.  Instead  of  rewriting  all  the  code  related  to  motion  coordination,  the  new  ditch  crossing 
coordinator  was  simply  added  to  the  original  Prolog  code.  In  order  to  accept  the  new  coordinator,  the 
original  plan  Prolog  goal  in  [16]  was  subdivided  into  two  plan  subgoals,  free  jolts  jnotlonjoordi- 
nation _plan  and  ditch_crossing_motion_coordinatlon _plan,  The  old  plan  code  in  [16]  was  merely  renamed 
as  free jaits_motion_coordination  _plan  without  any  furthei  modification,  and  the  new  ditch_crossing_mo- 
tion_coordinaiion _plan  code  was  simply  added.  If  the  original  motion  coordinator  logic  had  been  imbedded 
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in  the  "ASV"  robot  code  because  only  one  programming  paradigm,  such  as  an  imperative  paradigm,  had 
been  utilized  to  program  the  work  in  [IS],  the  extension  to  a  ditch  crossing  capability  in  the  "ASV"  robot 
code  would  have  been  a  very  difficult  and  very  time  consuming  task. 

Overall,  the  development  and  coding  of  the  new  extended  "ASV"  and  motion  coordinator  clearly 
manifested  the  advantages  of  the  use  of  multiple  programming  paradigms  to  program  a  complex  robot 
motion  coordination  function  which  constantly  performs  on-line  optimization  like  a  human  or  an  animal 
coordinating  his  or  its  motion  based  on  sensory  information  and  teamed  experiences.  Rule-based 
programming  to  express  logic,  object-oriented  programming  to  simulate  physical  and  functional  objects, 
and  a  numerical  processing  library  written  in  a  functional  or  imperative  language  to  im»»cment  mathematics 
and  physics  needed  for  simulation  are  very  naturally  divided  components  to  simulate  a  complex  system, 
such  as  that  treated  in  this  report. 

One  of  major  complains  about  programs  using  Artificial  Intelligence  techniques  and  languages  is 
slow  execution  speed  in  on-line  computing  applications.  In  this  study  reported  here,  the  most  prominently 
visible  candidate  to  be  blamed  for  slow  execution  speed  is  Prolog  code.  However,  the  execution  speed  of 
Symbolics  Prolog  on  a  Symbolics  Lisp  machine  is  not  so  slow  as  might  be  expected.  It  is  only  slightly 
slower  than  that  of  Symbolics  Lisp  or  Flavors.  However,  the  execution  speed  of  Prolog  implementations 
on  other  machines  are  usually  considerably  slower  than  those  of  non-Prolog  implementations.  One 
solution  for  slow  Prolog  execution  speed  may  be  to  use  a  special  Prolog  processor,  such  a  Xcnologic  X-l 
[27],  to  execute  Prolog  code.  The  other  solution  is  to  convert  the  Prolog  code  to  an  other  language,  such  as 
Lisp.  The  latter  approach  was  actually  adopted  to  test  the  correctness  of  the  program  developed  herein, 
using  a  TI  Explorer  machine  because  this  solution  is  readily  applicable  without  great  modification  to  the 
interface  between  Prolog  and  Lisp  codes,  and  because  a  TI  Explorer  machine  was  conveniently  available  to 
the  authors  in  an  office  environment.  Though  the  speed  gain  in  program  execution  is  little  over  that 
expected  with  Symbolics  Prolog,  this  approach  potentially  makes  a  much  wider  variety  of  computing 
hardware  suitable  to  execute  the  motion  coordination  program  developed  here.  Moreover,  this  approach  may 
provide  another  advantage  in  near  future  since  advances  in  microprocessors  based  on  RISC  or  CISC 
architecture  [28]  will,  with  respect  to  Lisp  execution  speed,  soon  equal  or  outperform  Lisp  machines  [20]. 
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Already,  with  respect  to  execution  speed  alone,  SPARC-based  Sun  workstations  narrow  the  large  gap 
previously  existing  between  a  Lisp  machine  and  a  conventional  machine  running  the  Lisp  language. 
Therefore,  rather  than  running  a  slow  Prolog  program  on  a  conventional  machine,  automatic  conversion 
from  Prolog  to  Lisp  after  a  development  phase  could  become  an  effective  way  to  achieve  markedly  better 
performance  if  the  program  were  to  be  tested  on  the  physical  ASV  walking  machine. 


7.  Summary  and  Recommendation 

The  main  putpose  of  this  study  was  to  demonstrate  the  value  of  a  multiple  programming  paradigm 
approach  in  the  development  of  software  for  motion  coordination  for  the  ASV  walking  machine.  An 
important  secondary  goal  was  extending  the  work  in  [16]  so  that  the  ASV  can  cross  a  ditch  without  any 
assistance  from  a  human  operator,  Thus,  the  terrain  handling  capability  of  the  ASV  under  program  control 
was  extended  from  binary-type  terrain  to  ternary-type  terrain  for  the  first  time.  The  third  goal  was  to  take 
into  consideration  the  overlapping  working  volumes  of  the  legs  of  the  ASV  in  order  to  utilize  the  full 
kinematic  capability  that  the  vehicle  geometry  can  give.  This  later  factor  made  a  direct  contribution  in 
widening  the  maximum  ditch  width  (MDW)  that  the  vehicle  can  cross. 

The  approach  adopting  multiple  programming  paradigms  for  motion  coordination,  which  was 
proposed  in  [1 1]  and  implemented  in  [16],  again  exhibited  its  power.  First  of  all,  it  forced  a  well-organized 
and  functionally  dean  abstraction  hierarchy  for  a  complex  and  ill-defined  problem.  Secondly,  it 
considerably  reduced  development  time  and  effort.  The  program  development  associated  with  this  report 
could  have  been  a  major  undertaking  if  a  single  programming  paradigm  had  been  utilized.  Instead,  as 
described  in  the  preceding  text,  with  the  approach  taken  here  most  of  the  program  in  [16]  is  reused,  white 
only  small  amount  of  code  is  additionally  written. 

The  code  translation  from  Prolog  to  Lisp  was  possible  because  the  Prolog  code  used  herein  was 
utilized  as  a  simple  rule-based  system.  This  success  of  this  translation  further  justifies  the  usage  of  Prolog 
as  one  of  the  languages  in  the  multiple  paradigm  environment  because  it  could  allow  much  wider  varieties 
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of  computing  hardware  to  execute  the  motion  coordination  program  developed.  Moreover,  this  actually 
made  the  program  execution  somewhat  faster  than  that  of  the  program  with  the  untranslated  Prolog  code. 

Among  studies  remaining  to  be  conducted  are  inclusion  of  vehicle  inertia  in  the  simulation,  effects 
of  leg  motion  on  the  location  of  the  vehicle  center  of  gravity,  and  a  better  simulation  of  the  vision  system. 
Such  a  study  would  be  appropriate  to  a  later  phase  of  this  research  along  with  an  investigation  of  further 
changes  to  the  Prolog  rule  set  to  enable  the  ASV  to  climb  over  large  obstacles  or  to  go  around  them  if  this 
is  not  possible. 
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Figure  1:  Typical  Simulation  Terrain  and  Vehicle 
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Figure  2:  Hierarchy  of  elmulation  objecta 
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Cycle  1  :State1 :  Place  Legs  in  the  Air 


■Cydfl1;State2;  BacbJMMaaa 


MalSlatfi2;  Forward  Rear  Legs 


•Cycle  1  :State4:  Forward  Middle  Legs 


.OydQl  ;State5;  Farward  Front  Legs 


Cvclal  :State6:  Lift  Middle  Leas  and  Move 


Figure  3:  Ditch  Crossing  Preparation  Phase 
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gydfl&Statal ;  Moya  Forward  Front  Legs 
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Cycle5;Stats3;  Move  Forward  Baac  Legs 


Figure  4:  Continued  ... 
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Mode;  PROLOG  Package;  robot-rulesBase:10 


robot  ;•  initialize,  repeat,  myjoop,  fail, 
initialize inits,  init_ditch_plan, 

init_dicth_plan  :•  retract(plan_cycle(J),  retract(plan_stateO),  fail. 
init_ditch_plan asserta(plan_cycle(l)), 

asserta(plan_8tate(placejegs„in_the_air)), 

my_loop  :•  get..command,  plan,  execute,  I. 

get_command :« X  is  read  Joystick. 


plan ditch_modc,  ditch_crosslng_motion_coordination_plan, 
plan free_gaiis_motion_coordination_plan 

dltch_mode  :•  dltch_mode(in),  jcleared  by  ditch_plan. 
ditchjnode  :•  X  is  at.ditch_arca,  Xnt,  asserta(ditch_mode(in)). 


execute  :•  execute„motion,  draw.robot, !. 
execute  jnotion X  is  execute_planned_motion, 
draw_robot X  is  graphical.display. 


Figure  5:  Prolog  Program 
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■)< if  ** i^i  *+ ** 41  ♦  ♦  *  if  if  if ♦  *1*1)11(1* **  *  %  if  *****  *  *  if  *+ ff  *♦* if  if  if  w *if  if  if  if  if  *  if  if 

Free  Gails  Motion  Coordination  Plan 


free_galts_niotion_coordination_plan  :•  update_robot_sute,  checkjkmjimit, 

leg.j)lan,  body_plan,  generate_decision,  I. 

update_roboi_siate X  is  update_roboLstatus. 


check_tkmjimit  :•  AJeg  is  atjkmjimit,  AJeg  W  nil, 
asserta(limitJeg(AJeg,lift)). 

checkjkmjimit. 

leg_plan  :•  lift_tLleg. 
leg_plan  :•  exchange Jegs, 
leg_plan  :•  stable, 
legjplan  :•  place.ajcg. 
legjplan  i*  waitjorjegs. 

stable Condition  is  stable_p.  Condition  «■  L 

lift_a_leg  :•  stable,  AJeg  is  smallest. tkm.  leg,  AJegV-  nil, 

Condition  is  stable.wlthout(AJeg),  Condition  ■■  t, 
asserta(decision(AJeg,_,lift)). 

exchangejegs  :•  stable,  LegA  is  smaliestjkmjeg,  LegA  W  nil, 
LegB  is  max_smJeg(LegA),  LegB  W  nil, 
Condition  is  has_more_tkm(LegB  JLegA), 
Condition  »«•  t, 

asserta(dccisionCLegAJLegB,  exchange)). 

place.ajeg  :•  A_leg  is  max_sm_leg(J,  AJeg  W  nil, 
asscrta(decision(AJeg,_, place)). 

waitjorjegs  :■  try.newjoothold, 
waitjorjegs  :•  recovery,  asserta(reduce_spccd). 
waitjorjegs asserta(reducejpeed),  res  tore  Jimit_lcg. 

try.newjoothold  AJeg  is  leg_with_newJoothold,  AJcg\*«  nil, 
asserta(decision(AJeg,_,  place)). 

recovery  :■  AJeg  is  do.recovcry,  AJeg  vw»  nil, 

asserta(decision(AJeg,_,  place)),  restore  JimitJcg. 

restorejimitjeg  :>  »«tract(limit_leg(AJeg,lift)). 
restore_limit_leg. 


Figure  5:  Continued ... 
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******** ************************************************** 


;;  Ditch  Crossing  Motion  Coordination  Plan 


ditch_crossingjnotion_coordination_plan ditch_plan_done,retract(ditch_mode(in)),  idle_cyclc. 
dltch_crossing_motion_coordination_plan cycle_planner. 


Cycle  Planner 


ditch_plan_done plan_cycle(6),  retract(plan_cycle(6)), 
asscrta(plan_cycle(  1)), 
prepare_next_ditch_plan, 

prepare_next_ditch_plan move. 

cyclc_planncr one_cycle_done,  plan_cyclc(N),  N1  is  N+l, 

retract(plan„cycle(N)).  asserta(plan_cycle(N  1 )), 
idlo.cyclo. 
cyclejilanncr plan_cycle. 


** 

!!***«***•*•  cyCie  Dispatcher  ************ 

• » 

one_cyclc_donc  :■  plan_statc(onc_plan_cyclc_donc), 

retractO)lan_state(onc_plan_cycle_donc)), 
initial  ize_plan_statc . 


initializc_plan_statc  :•  asserta(plan_statc(stan)). 


plan_cyclc  :•  plan_cycle(l),  update.roboLatale,  ditch_plan_cycle_l , 
body_plan,  gencrate_decision,!, 

plan_cyclc  :•  plan_cyclc(2),  updatc_robot_sute,  diich_plan_cycle_2, 
body.plan,  generate_decision,!. 

plan_cycle  :•  plan_cycle(3),  updatc_robot_state,  ditch_plan_cyclc_3, 
body.plan,  generate.decision.l. 

plan_cycle  :■  plan_cycle(4)l  update_roboLstate,  ditch_plan_cyclc_4, 
body_plan,  generate_decision,l. 

plan_cycle  :•  plan_cycle(5),  updatc_robot_statc,  ditch  _plan_cycle_5, 
body_plan,  gencrate_decision,l. 

Idlc_cycle  :■  updatc_robot_statc,  body_plan,  gencratc_declslon, !, 


Figure  5:  Continued 
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Cycles  ********* 


ditch_plan_cycle_l  :•  plan_sune(stan),  rciract(plan_siate(sian)), 
asserta(plan_state(place_legsJn_the_aLr)). 
place  Jegs_in_thcjur(back_,middleJegs), 
ditch_plan_cycle_l  :•  place Jegg_injhe_air(tek_middleJegs). 
ditch_plan_cyele_l back_mlMe_legs(forwafd_rearJegs). 
ditclLplaiLcycleJ  :*  forwardjearJegs(fcrwwd_middle_legs). 
ditch_plan_cycle_l  :•  forward_middejegs(forward_&ontjegs). 
ditch_plan_cycle_l  :•  forward_frontJegs(UJPtjniddleJegs_aiKl_mGve). 
ditch_plan_cyde_l liftjnlddeJegs_and_move<one_plan_cycle..done). 


ditch_plan_cycle_2  :•  plan_state(stan),  retract(plan_state<siart)), 
aaserta(plan_state(back_middlc_legs)), 
back_middleJegs(fonvard_rear_legs). 
ditch_plan_cycle_2 backjniddieJcgs(forward_rearJegs). 
ditch _plan_cycle_2 forward_rearJegs(forwafd_middle_legs). 
dltch_plan_cycle_2  :*  forward_mlddleJegs(one_plan_cycle_done). 


ditch_plan_cycle_3  :•  plan_state(start),  retract(plan_state(start)), 

asserta(plan_state(move_forward.fronLlegs)), 
move_forward_ffont.legs(move_forward_middlojcgs). 
ditchj>lan_cycle_3  ;•  move_forward_fronLl«gs(move_back_middlc_legs). 
ditch_plan_cycle_3 !-  move_back..middle_legs(move_forward_rear_legs). 
ditch_plan_cyclc_3  ;*  move_forward_rear_legs(one_plan_cycle_done). 


ditch_plan_cycle_4  :•  plan_state(start),  retract(plan_state(start)), 

asserta(plan_statc(move_forward_mlddIe_legs)), 
move_forward_middleJegs(one_plan_cycle_donc). 
ditch_plan_cyclc_4 move_forward_mlddle_legs(one_plan_cycle_done). 


ditch_plan_cycle_5 plan.state(start),  retract(plan_statc(start)), 

asscrta(plan_state(move_forward_front_lcgs)), 
movc_forward_frofiUcgs(move_forward_middlc_legs). 
ditch_plan_cycle_5  :•  moveJbrward_tronLlegs(move_back_rniddleJegs), 
ditch  j>lan_cycle_5 move_back_middlejegs(move_forward_rear_legs). 
ditcti_plan_cyclc_5  :■  movcjorward_tear_legs(oncj)lfln_cyclc_donc). 


Figure  5:  Continued  ... 
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Stales  *************** 


;;;;;  back_middle_legs  subgroup 

back_midd]eJegs(Ncxt_Siate) plan_state(back_m  I  ddJe  Jegs) , 

back_middle_legs_done, 

retract(plan_state(back_middle_lp,gs)), 

asserta(plan_state(Next_State)), 

stop. 

back_middle_legs(Next_State) plan_state(back_mlddlejegs), 

do_back_middleJegs, 

stop. 


!!!.!  forward_front  Jegs  subgroup 

forward_front_lcgs(Next_Statc) plan_state(fonvard_frontJegs)), 

forwardjront_lcgs_donc, 

retract(plan_state(forward_fronLlegs), 

asserta(plan_state(Next_Stflte)), 

stop. 

forward JrontJegs(NexLState)  ••  plan_state(forward_front_legs)), 

doJbrwanLfrontJegs, 

stop. 


;;;;;  forward_mlddleJegs  subgroup 

forward_middleJegs(Next_State)  :•  plan_state(forwardjniddleJegs), 

forward jniddleJcgs_done, 
rctract(plan_state(forwardjniddlcJcgs). 
asserta(plan_state(Next_State)), 
stop. 

forward_mlddleJegs(Ncxt_Statc)  :■  plan_statc(forward._mlddleJcgs), 

doJbrwand.midcUcJcgs, 

stop. 


;;;;;  forward_rear_lcgs  subgroup 

forward_rear_lcgs(Ncxt_Stale)  :•  plan_state(forward_rear_legs), 
forward_rear Jegsjlonc , 
retract(plan_stali(forward_rear_legs), 
asscrta(plan_statc(Ncxt_Statc)), 
stop. 

forward_rearJegs(Next_State) plan_state<forward_rearJegs), 
do_forward_rcar_kjgs, 
stop. 


;;;;;  !ift_middle_lcgs_and_movc  subgroup 

lift_mlddlcJcgs_and_move(Ncxt_Stfltc) plan_statc(lift_middlcJegs_and_movc), 

move_done,  stop, 

Figure  5:  Continued ... 
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retract(pUn_stulfi(Uft_middle_lcgs_and  .move)), 
assena(plan_state(N  ex  t_S  tate)) . 

Uft_middleJegs_«nd_move<NexLSiate) plan_state(l  ift_middle_legs_and_mo  ve) , 

do_lift_middle  Jegs,  move. 


mil  move  .back_middle Jegs  subgroup 

move J>ack_middle Jegs(Next_S  tate)  :*  plan_state(move_back_middle Jegs) , 

move_beck_middleJegs_donc, 

reuact(plan_sUUe(move_back_middle_legs))1 

asseita(plan_state(NexLState)). 

moveJ«ck_middleJegs(Next_Slate) plan  jtate(moveJ»ck_middleJe£s), 

do_moveJ»ckjniddleJegs. 


;;;;;  movej'orwardjfrontjegs  subgroup 

moveJorward_£rontJegs(Next_State) plan_state(nK>ve  Jorward  JtontJegs), 

move_forward_&om_legs_donc, 
retract(pUin_state(move_forw0rd_front_legs)), 
asserta(plan_state(Next.  .State)). 

move_forM'ard_£ronUcgs(NexLState) plM_staie(ftroveJbrwardJrontJegs), 

do_"iove_forward_frontjegs. 


;;;;;  move_forward_middleJegs  subgroup 

move_forward_middle_legs(NexLState) plan_state(moveJbrward_mlddleJegs), 

move_forward_middleJegs_doi», 

rettact(ptan_state(«nove_forwerdj*rontJegs)), 

asserta(plan_state(NextJ!tate)). 

moveJbrward_middleJcgs(Next_State)  :•  plan_state(move_forwtuxl_middleJegs), 

do_move_forwaid_middle_legs. 


;;;;;  move_forward_rear_legs  subgroup 

move Jorward_rearJegs(Next_State) plan_state(move_forward_rearJegs), 

move_forward_middle_lcgs_donc, 
retract(plan_statje(move_forward_rear_lcgs)), 
asserts  (plan_staic(Ncxt_State)). 

move_forward_rear_lcgs(Ncxt_State) plau_state(move_forward_rcar_legs), 

do_move_forward_rear_legs, 


;;;;;  place  Jegs  Jn_the_air  subgroup 

place Jegs_in_the_air(Ncxt_Statc)  :•  plan_state (place Jegs_tn_the_air), 

place  JegsJnjhe_air_dnne, 
retract(plan_state(place  Jegs..in_the_air)j , 
asserta(plar_state(Ncxt_state)), 
stop. 

place  Jegs_in_the_air<Next_Slate)  :•  plan_state(place  Jegs_in_the_air), 

do_place_legs_in_the_air,  slop. 


Figure  5:  Continued ... 
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state  Executors 

ft 


;;;;;  doJ»ck_middleJegs  subgroup 

back_middleJegs_done  :•  all_middle_legs_lifted1  all_mlddlejegs_placed, 

clear _middleJifted_memoiy,  clear_move_memory. 
do_back_m  iddle Jeg  s all_middlejegs_lifted,  place_middlejegsj»ck. 
do_back_middleJegs  :•  lifi_mlddlejegs. 

all_middlejegsjlfted middle Jegs(lifted), 
all_middiejegsjifted X  is  bothjniddlejegsjifted,  X  ■■  t, 
asseita(middlejegs(lifted)). 

all_middle_legs_p  laced  :•  X  is  both_middleJegs_placed,  X  ■■  t. 

clearjniddlejifted.  memory retract(middleje8s(lifted)). 

place_middlcjegs_back  :>  AJeg  is  placable_middlejeg,  AJeg  V*  nil, 
asserta(decision(A_legi_,place_back)), 

place_middle_legs_back. 

Hft.middlejegs A. leg  Is  liftable_mlddle_leg,  AJeg^*»  nil, 
asserta(decision(Aj5g,_,lLft)). 

lift_middlejegs. 


;;;;;  dojorwardjrontjegs  subgroup 

forward_front_legs_done  :*  alLfrontJegsJifted,  all_frontJegs_placed, 

clear JrontJiftedjnemory.clearjnovejnemory. 

do_forward_frontJegs  :•  all_frontJegs_liited,  placc_frontJegs. 
do_forward_irontJegs  lift_front_legs. 

alLfronlJegsJiftcd  :•  front_lcgs(lifted). 
alLfrontJegsJifted X  is  bothjromjegsjifted,  X  *=*  t, 
asserta(frontJegs(liftcd)). 

all_fronUegs_placed X  is  both_firont  Jegs_placed,  X  ■■  L 

clearjfrontJiftedjTiemory retraci(front_legs(lifted)). 

place.. front Jegs AJeg  is  placable_front_leg,  AJeg  nil, 
asserta(decision(AJeg,_,  place)). 

place_frontJegs, 

1  if t_front_legs  Ajeg  is  liftable.frontjeg,  AJeg  V*  nil, 
asserta(decision(A_leg  lift)). 

lifl_front_legs. 
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;;;;;  do_forward_middlc_legs  subgroup 

forwaid_(niddleJegs_donc  :•  all.„middlejegs_lifted,  all_middleJegs_pUced, 

clearjniddleJiftedjnemory,  clear_movc_mcniory, 

doJorward_middle_legs all_middlejegs_lifted,  placc_middlejegs. 
do  Jorward_middleJegs  lift_middlejegs. 

place.middlejcgs A_lcg  is  placable_middlejeg,  AJeg  W  nil, 
asserta(decision(Ajeg^., place)). 

place jniddlejegs. 


»!>>!  dojorwardjear _lega  subgroup 

forwardjear_legs_done  :•  alljearjegsjlfted,  all_rearjegs_placed, 

clear_rearJifted_memory,  clear_move_memory, 

do_fofwwd_rearJegs alljTontJegsJifted,  placejrearjegs, 
do_forward_rcarJegs llftjearjegs. 

aILrearJegs_lifted rear Jegs(liAed). 
alLrear_legs_lified  :■  X  is  both_rear Jegs  Jif  ted,  X  «■  t, 
asseita(rearjegs(lifted)). 

alljear _legs_placed X  is  both_rcarJegs_placed,  X  «u  t. 

clear jearJiftedjnemory  !•  retroct(reai  Jegs(lifted)), 

place_rear Jegs  :■  AJeg  is  placable_rear Jeg,  A  Jeg  nil, 

asserla(decision(AJegv_, place)), 

placejearjegs. 


liftjrearjcgs AJeg  is  lifiable_rearjeg,  A  Jeg  V-  nil, 
assem(decision(A_leg  ,_,lift)). 


lifl_rcar_legs. 


;;;;;  doJtft_middleJegs  subgroup 
do_lifLroiddleJegs liftjniddlejegs. 


;;;;;  do_move_back_middleJcgs  subgroup 

move_back_middlejegs_done alljniddlejegsjifted,  all_middle_legs_placed, 

ciear_middle_ilfied_memory,  cIear_move_memory, 
stop. 

do_move_back_m  iddle_legs alLmiddleJegsJifted,  move_done,  stop, 

place_middlejegs.  back. 

do_move_back_middleJegs all_mlddlejegsjifted,  move. 
do_move_back_middleJegs  :•  lift_middlc  Jegs,  stop. 


Figure  5:  Continued ... 


40 


;;;;;  do_movc  _forward_frontJcgs  subgroup 

movejbrward_ftont_lcgs_done  :•  alljfromjegsjifted,  all_ftontJegs_placed, 

clear_frontjifted_memory,  clear_move_memory, 
stop. 

do_move_forward_frontJegs  :■  all_frontJegsJiftcd,  move  .done,  stop, 

place JrontJegs. 

do_move_forward_front_legs  :»all_frontJcgsJifted,  move. 
do_fnove_forward_front_legs lift_front_legs,  stop. 


;;;;;  dojnoveJorwardjniddleJegs  subgroup 

'  move_forward_rniddle Jegsjkxie  :■  all_middle Jegs.  lifted , 

alljn  iddle Jegs_  placed, 
clear_middleJlfted_memory, 
clear_move_memory,  stop. 

dojnoveJbrward_middle_legs  :•  all_middle_legs_lified,  movc_done,  stop, 

place_mlddle_legs. 

do_move_forward_middlejegs  :•  all  jniddlejegsjlfted,  move. 

do_move_forward_middleJegs :« lift_midd]e_legs,  stop. 


iliii  do_move_forward_rear_legs  subgroup 

move_forwaid_rearJegs_done :» alLrearJegs  Jifted,  all_tearjegs_placed, 

c!ear_rearJlfted_mcmory,  clear_move_memory, 
stop. 

dojnove.forward^rearjegs alljrearjegsjlfted,  move,  done,  stop, 

piace_rearjegs. 

do.move_forward_rear_legs alljearjegsjiftcd,  move. 
do_movc_forward_rearJegs  :<•  lift_rearjegs,  stop. 


;;;;;  place  _legs_in_the_aii  subgroup 

place_legs_in_the_air_done X  is  alljegs_placed,  X  «■  l 

place  Jegs_in_thc_air  :•  AJeg  is  placable Jeg,  A_leg  W  nil, 
asserta(decision(AJcg,_,  place)). 

place  JegsJn.  thc_air. 


;;;;;  body_movement  subgroup 

move asseita(resume_movei.ient), 
stop asserta(stop_movement). 

clear_move_memory  :•  retract(move<done)), 
clear_move_memory. 

move.done  :•  move(done). 

movc_done  :•  X  is  at_tkm  Jlmit,  X  nil,  asserta(move<done)). 
move_donc X  is  at_stability  .limit,  X  V«  nil,  asserta(move(done)). 

Figure  5:  Continued  ... 


41 


,,•*•*•**+•**«■****»******»+»****»*+****•****»*+*****»**»»»* 

If 

II 

;;  Plan  Libraries 


;;;;;  body _plan  subgroup 

body_plan  :•  speed_plan,  trajectory  _plan. 

speedjslan  :■  retracKreduce_specu>,  slow.down. 
speed_plan apeed_up. 

speed.up  X  is  speed_up_robot. 

slow_down X  is  slow_down_robot. 

tralectoryjplan  :•  stable_m,  restore  jntfectory. 
trajectory _plan modifyjr^jectory, 

stablejn  Condition  is  stable_p_m,  Condition  ■■  t. 

restorejrujectory X  is  restore.command. 

modifyjrajectory  :•  X  is  modify.command. 


;;;;;  generate_declslon  subgroup 

generaie_declsion retract(decigion(A  Jeg,B  Jeg,A_decision)), 

X  is  send_decision(AJeg,B  Jeg,A_decision),  fail. 
generate_decislon retract(llmiLleg(A  Jeg,A_decision)), 

X  is  send,_decision(A  Jeg,_,A_decislon),  fail. 

generate_decislon. 
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Appendix 


Lisp  Code  for  ASV  Simulation 


body-cont rollsr-t , lisp  Wad  Nov  28  10:11:54  1980  1 

til  Mode iComnon-Liap;  Baae:10 

;****»«*«**»«**********************«**•***********«**•******»***** 

; 

i  body-contcoll«r  definition 

; 

;*************************************H********lll****************** 

(def flavor  body-controller ( joyatick-command-regulator  terrain-regulator 

H-calculator 

body-trana -ratal  body-rot ate-ratel 
body-trans-rataS  body-rotate-rate 6 
body-trana-ratelO  body-rot ate-ratel 0 
HI  inv-Hl  HS  inv-H6  H10  inv-HIO 
H  inv-H  body-t  body-r) 


0 

t initable-lnat ance-var iablea ) 


(defmethod  (body-controller  ilnitti) 

0 

(aetf  joyatick-command-regulator  (make-inatance  ' joyatick-oommand-regulator) ) 
(aetf  terrain-regulator  (make-inatanoe  'terrain-regulator)) 

(aetf  H-calculator  (make-inatanoe  ' H-oalculator) ) 

(aend  joyatick-oommand-regulator  linitti) 

(aend  terrain-regulator  linitti) 

(aetf  H  (aend  H-caloulator  linitti)) 

(aend  aelf  linit-body-ratea) 

(aend  aelf  tinit-H) 

HI 

) 


(defmethod  (body-controller  linit-body-ratea) 

0 

(aetf  body-trana-ratel  ' (0  0  0) ) 

(aetf  body-trana-rateS  '(0  0  0)) 

(aetf  body-trana-ratelO  '  (0  0  0)  ) 

(aetf  body-rotate-ratel  '  (0  0  0) ) 

(aetf  body-rotate-ratel  ' (0  0  0)  ) 

(aetf  body-rotate-ratelO  '(0  0  0)) 

) 


(defmethod  (body-controller  iinit-H) 
0 

I  library  fucntion  :  ident 
(aetf  HI  H) 

(aetf  H6  H) 

(aetf  H10  H)  , 

(aetf  inv-H  (matrixinv  H)) 

(aetf  inv-Hl  inv-H) 

(aetf  inv-H6  inv-H) 

(aetf  inv-HIO  inv-H) ) 


body-controller-t . lisp 
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(defmathod  (body-controller  : control) 

( joystick-command  deceleration-factor  estima tad- support -plane) 

(setf  H  Kl) 

(send  self  :upd«te  joystick-command  deceleration-factor  estimated-support-plane) 
(send  self  :$ava) 

(dotimes  (i  10) 

(oond  ((equal  i  0) 

(setf  body-trans-ratel  body-t) 

(setf  body- rot ute-rat-el  body-r) 

(setf  HI  H) 

(setf  inv-Hl  i.iv-H)) 

( (equal  i  S) 

(setf  bodv-trans-rate6  body-t) 

(setf  body-rotate-rate6  body-r) 

(setf  H6  H) 

(setf  inv-H6  inv-H)) 

((equal  i  9) 

(setf  body-t rans- ratal 0  body-t) 

(setf  body-rotste-ratelO  body-r) 

(setf  H10  H) 

(setf  inv-H10  inv-H) ) ) 

(send  self  (update  joyatick-oommand  deoaleration-faotor  estimated-support-plane) 
) 

(send  self  irastora)) 


(def method  (body-oontroller  i update) 

( joystick-command  deceleration-factor  estimated-support -plane) 
;  internally  used  by  control  method 

(let*  ((t-command  (send  terrain-regulator  : regulate 

estimated-support-plane  H) ) 

(j-command  (send  joystiok-command-regulator  t regulate 

joystiek-eommand  deceleration-factor) ) 

) 

(setf  body-t  (list  (first  j-command)  (second  j-oommand) 

(third  t-command) ) ) 

(setf  body-r  (list  (first  t-command)  (second  t-command) 

(third  j-command) ) ) 

(setf  H  (send  H-calculator  :new-H  body-t  body-r)) 

(setf  inv-H  (matrixinv  H) ) ) ) 


(defmathod  (body-controller  : restore) 

0 

/  internally  used  by  control  method 

(send  joystiok-command-regulator  s restore) 
(send  terrain-regulator  (restore) 

(send  H-oaloulator  (restore)) 


(defmethod  (body-controller  (save) 

O 

;  internally  used  by  control  method 

(send  joystick-command-regulator  (save) 
(send  terrain-regulator  (save) 

(send  H-calculator  (save)) 


body-controller-t . lisp 
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(defmethod  (body-controller  ;get-body-trana-ratel) 

0 

body-trana-ratel) 


(def method  (body-controller  iget-body-rotate-ratel) 
0 

body-rotate-ratal) 


(defmethod  (body-controller  tgat-body-trana-ratelO) 
0 

body-trana-ratel 0) 


(defmethod  (body-controller  iget-body-rotate-rateiO) 

0 

body-rotate-ratelO) 


(defmethod  (boay-controller  :get-Hl) 
0 

Hi) 


(defmethod  (body-eiontroller  iget-inv-Hl) 
0 

inv-Hl) 


(defmethod  (body-controller  :get-Hfi) 
0 

H6) 


(defmethod  (body-controller  :get-inv-H6) 

0 

inv-H6) 


(defmethod  (body-controller  sget-HlO) 
0 

H10) 


(defmethod  (body-controller  iget-inv-H10) 
0 

inv-HIO) 


body-t . lisp 


Wad  Nov  28  10:11:59  ’990 
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III  -*-  Mode . •Common-Lisp;  Base: 10  -*- 


;  body  flavor  definition 
) 


(def flavor  body (stability-calculator  aupport-plane-eatimator 

body-controller  owner 
estimated-support-plane 
daeelaration-f actor 
support -plane-clock 
modify-vector 
modify-vector-p 
stop-motion-flag 
joy-command) 

0 

linitable-instance-variables) 

i 


(defmethod  (body  i slow-down) 

0 

(setf  deoeleration-factor  (+  deceleration-factor  1)) 
(if  (>  deoalaration-faotor  20) 

(setf  deoeleration-factor  20))) 

(defmethod  (body  : speed-up) 

0 

(setf  deoeleration-faotor  (-  deceleration-factor  1)) 
(if  {<  deoeleration-factor  0) 

(setf  deoeleration-faotor  0))) 


(defmethod  (body  :atable-m) 

( supporting-legs ) 

(send  stability-calculator  istable-m 

supporting-legs  (send  Body-controller  ;get-H10))> 


(defmethod  (body  istable-p-m) 

(supporting-p-legs  a-leg) 

(send  stability-calculator  :stable-p-m 

supporting-p-legs 

(send  body-controller  iget-Kl))) 


(defmethod  (body  :stop-p) 

0 

(let  ((trans-rate  (send  pelf  :get-body-trans-ratsl) ) ) 
(equal  (list  (first  trans-rate) 

(second  r.rana-rite) ) 

'  (0.0  0.0)))) 


(defmethod  (body  imodify-commanrv 
0 

(setf  modify-vector 
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(sand  stability-ealculatox  ;get-recovery-vector) ) ) 


(dafmathod  {body  imodi£y-command-p) 

0 

(aatf  modify-vactor-p 

(aand  atability-calculator  igat-racovary-vactor-p) ) ) 


(dafmathod  (body  sraatora-command) 

0 

(aat£  modify-vaetor  '(0  0  0))) 


(dafmathod  (body  jraatora-oommand-p) 

0 

(aatf  modify-vactor-p  MO  0  0))) 


(dafmathod  (body  latop-motion) 
<a-lag) 

(aatf  atop-motion-flag  flag)) 


(dafmathod  (body  iraatora-motion) 

0 

(aatf  atop-motion-flag  nil)) 


(dafmathod  (body  linitti) 

0 

(aatf  dacalaration-faotor  0) 

(aatf  modify-vaotoe-p  M0  0  0)) 

(aatf  modify-vaator  M0  0  0)) 

(aatf  atop-motion-flag  nil) 

(aatf  aupport-plana-olook  10) 

(aatf  atability-aalculator 

(maka-inatanoa  ' atability-calculattor) ) 

(aatf  aupport-plana-aatimator 

(maka-inatanoa  ' aupport'-plana-astimator) ) 
(aatf  body-oontrollar 

(maka-inatanoa  'body-oontrollar) ) 

(aand  stability-aaloulator  linitti) 

(aand  aupport-plana-aatimatcr  sinioti) 

(aand  body-oontrollar  linitti) 

) 

(dafmathod  (body  :gat-modify-vaotor) 

O 

(vaotaub  modify-vactor 

(dotprod  modify-vaotor 

(normaliaa-vaotor  joy-oommand) ) ) ) 


(dafmathod  (body  :gat-modify-vector-p) 
0 

modJ.fy-vactor-p) 
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(defmethod  (body  tcslculete-motion) 

(joystick-command  legs) 

(setf  joy-ccronand  joystick-command) 

(cond  ((equal  support-plane-olock  10) 
l  ???  bug  ??? 

(setf  estimated-support-plane 

(send  support-plane-estimator  ;get-plane  legs)) 

(setf  support-plane-elock  OH) 

(setf  support-plane-olook  (+  aupport-plane-elook  1)) 

(oond 

((or  stop-motion-flag  (null  modify-vector-p) ) 

(send  body-controller  soontrol 
' (0  0  0) 

0  estimated-support-plane) ) 

(modify-vector-p 
(send  bcdy-oontroller  soontrol 

(vectadd  joy-command  (send  self  iget-modify-veotor-p) ) 
deceleration-faotor  estimated-support -plane) ) 

(t 


(control  body-controller 

(vectadd  joy-command  (send  self  iget-modify-vector) ) 
deceleration-factor  estimated-support-plane) ) ) ) 


(defmethod  (body  iget-eatimated-aupport-plane) 


0 

estimated-support-plane) 


(defmethod  (body  iget-hody-trans-iratel) 

0 

(send  body-controller  iget-body-trane-rstel) ) 


(defmethod  (body  iget-body-rotate-ratel) 

0 

(send  body-controller  iget-body-rotate-ratel)) 


(defmethod  (body  :get-body-trans-ratelO) 

O 

(■end  body-controller  iget-body-trens-retelO) ) 


(detmethod  (body  :get-body-rotate-retalO) 

0 

(■end  body-controller  iget-body-rotete-retelO) ) 


i 

(defmethod  (body  : get -HI) 

0 

(send  body-controller  sget-Hl)) 


(defmethod  (body  iget-inv-Hl) 


i 
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0 

(send  body-controller  :g#t-inv-Hl) ) 


(defmethod  (body  :get-H6> 

0 

(send  body-controller  iget-H6)) 


(defmethod  (body  :get-inv-H6) 

0 

(send  body-oontroller  :get-inv-H6) ) 


(defmethod  (body  iget-HIO) 

0 

(send  body-controller  iget-HIO)) 


(defmethod  (body  :get-inv-HlO) 

0 

(send  body-controller  iget-inv-H10) ) 


(defmethod  (body  more-eteble) 

(supporting-legs  legl  leg2) 

(send  etability-oaloulator  nnore-steble 

supporting-legs  (send  body-oontroller  :get-H10) 
legl  leg2) ) 


(defmethod  (body  : stable) 

(supporting-legs) 

(send  stability-calculator  :stable 

supporting-lags  (send  body-controller  :get-H10))) 


(defmethod  (body  tstable-p) 

(supporting-p-legs) 

(send  stability-caloulator  :stable-p 

supporting-p-legs  (send  body-controller  iget-Hl))) 


i 


1 


command-xegulator-t . lisp  Wed  Nov  28  10:11:34  1990 

lit  -*-  Mod# ! Common-Lisp;  Base: 10  -*- 

. ********************************************************** 
l 

i  regulator  flavor  definition 
; 

j********************************************************** 


(defflsvor  regulator ( (max-a  3.2174)  (time-oonst  0.5)  (sampling-time  0.1)) 
0 

:  initable-instance-variables ) 


(defmethod  (regulator  i filter) 

(desired-x  present-x) 

;  first  order  regulation 

(let  ( (del-vel  (/  (-  desired-x  preaent-x)  time-const))) 
<  +  (*  (send  self  :g-limitor  del-vel)  sampling-time) 
present-x) ) ) 


(defmethod  (regulator  :g-limitor) 

(del-vel) 

;  limit  aooeleration  to  3.2174  ft/(aeo*seo)  or  0.1  0. 
(oond  (<>  del-vel  max-a)  max-a) 

((<  del-vel  (-  max-a))  (-  max-a)) 

(T  del-vel) )) 


; 

i  joyatiok-command-regulator  flavor  definition 

i 


(defflavor  joyatick-command-regulator (body-trans-rate-x 

body-trans-rate-y 
bcdy-rotate-rate-z 
old-body-trans-rate-x 
old-bodytrans-rate-y 
old-body-rotate -rate-*) 


(regulator) 

:  initable-instance-variables) 


(defmethod  ( joyatick-command-regulator  sinitti) 

0 

(setf  body-trans-rate-x  0.0) 

(aetf  body-trans-rate-y  0.0) 

(setf  body-rotate-rate-z  0.0) 

(list  body-trans-rate-x  body-trans-rate-y  body  rotate-ratQ-z) ) 


command-ragulator-t  .lisp 
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(rfefmuthod  ( joystick-command-regulator  tregulatt) 

(joystick-command  deceleration-factor) 

(if  (<-  deceleration-factor  0) 

(setf  deceleration-factor  0.5))  /  remove  effect  of  doceleration-factu 

(let*  ( (d-const  0.5) 

(x  (*  (first  joystick-command)  (/  d-const  deceleration-factor))) 

(y  (*  (second  joystick-comrr.and)  {/  d-const  deceleration-factor))) 

(r  (*  (third  joystick-command)  (/  d-const  deceleration-factor)))) 

(setf  body-trana-rate-x  (send  self  $  filter  x  body-trana-rate-x) ) 

(oetf  body-trans-rate-y  (send  self  ifiltar  y  body-trans-rate-y) ) 

(setf  body-rotate-r&te-z  (send  self  ifiltar  r  body-rotate-rate-*))) 

(if  (<  (aba  body-trana-rate-x)  0.02)  (setf  body-trans-rate-x  0.0)) 

(if  (<  (aba  body-trans-rate-y)  0.02)  (setf  body-trans-rate-y  0.0) 

(if  (<  (aba  body-rotate-rate-z)  0.005)  (setf  body-rotate-rate-z  0.0)) 
(list  body-trans-rate-x  body-trans-rate-y  body-rotate-rate-z)) 


(defmethod  ( joystick-command-regulator  :restore) 

O 

(setf  body-trans-rate-x  old-body-trans-ratn-x) 

(setf  body-trans-rate-y  old-body -trans-rate-y) 

(setf  body-rotate-rate-z  old-bcdy-rotate-rate-z) 

(list  body-trans-rate-x  body-trans-rata-y  body-rotate-rate-z)) 


(defmethod  < joystick-command-regulator  tsave) 

O 

(setf  old-body-trans-rate-x  body-tran*  ■■  rate-x) 

(setf  old-body-trans-rate-y  body-trans-rate-y) 

(setf  old-body-rotata-rate-z  body-rotate-rate-z) 

(list  body-trans-rate-x  body-trans-rate-y  body-rotate-rate-z)) 


l 
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in  -*-  Mods : Common-Lisp;  Bass: 10 


; 

i  state  flavor  definition 
; 

» ************************************************************** 

(daf flavor  atate(name  next .ate) 

O 

: initable-instance-variables) 


(dafmethod  (state  : state-name) 

0 

name) 


(dafmethod  (state  s set-next-stata) 

(a-atate) 

(setf  next-state  a-at«ta)) 


fa*********************************************************** 

; 

;  sync-state  flavor  definition 

i 

;*************************************************«********** 


(defflavor  sync-state ( (time  0)  (del-t  0.1)  time-out) 
(state) 

: initable-instanca-variables) 


(defmethod  (sync-state  : change) 
0 

(setf  time  (+  time  dsl-t)) 
(cond  ((>■  time  time-out) 
(setf  time  0) 
next-state) 

(t  self) ) ) 


(defmethod  (syno-state  : get-time) 
0 


i 


time) 
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f 

i  async-state  flavor  definition 
; 

a***************************************************  *  *  *  *  *  *  * 


(def flavor  async-state ( (command  nil)  (obaarvaticn  nil)) 
(State) 

: initnble-ihst anca- variables ) 


(dafmathod  (async-state  : change) 

(given-command  observed -event) 

(oond  ((and  (not  observation) 

(equal  given-command  command) ) 
next-state) 

( (and  (not  command) 

(equal  observed-event  observation) ) 
next-state) 

(t  self) ) ) 


;***.■**  *******  ***  *******  ***************6**  **#  #***#****#*** 

; 

I  state-machine  flavor,  definition 
/ 

;*****w********#tv*******************#***************»**** 


(def flavor  state-machine (state  owner) 
O 

: initabla-inatance-variables ) 


(dcfmethod  (state-machine  'State-name) 
0 

(send  state  :state-name) ) 


j****«******«#*******#*************************«******** 

; 

;  oontrol-state-maohine  flavor  definition 

I 

;****************************************«************** 

(def flavor  control-state -irfachine ( (command  nil)  (observation  nil) 

contact-sensor  executor* 

(statu -machine) 

: initable- instance-variables ) 
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(defmethod  (control-state-machine  iinitti) 

(leg-name) 

(if  (member  leg-name  ' (legl  leg4  leg5)> 

(send  self  :init-control-maehine  'support) 
(send  self  : init-oonttol-machine  'ready)) 

(setf  contact-sensor  (send  owner  :contact-sensor) ) 
(setf  executor  (send  owner  iexecutor))) 


(defmethod  (contsol-state-maehine  iinit-control-maohine) 

(a-state-name) 

;  internally  used  by  init  method 

(let  (return  lift  support  oontaet  descent  advance  ready) 

(setf  return 

(make-instance  'sync-state 

:nama  'return  : time-out  0.6)) 

(setf  lift 

(make-instance  'sync-state 

mama  'lift  s time-out  0.4 
mext-atate  return)) 

(setf  support 

(make-instance  'async-state 

mama  'support  t command  ' taeovar-command 
mext-state  lift)) 

(setf  contact 

(make-instance  'sync-state 

mama  'oontaet  : time-out  1.0 
mext-state  support)) 

(setf  descant 

(make- instance  'aaync-state 

mama  'descent  {observation  'contact-oonfira 
mext-state  contact)) 

(setf  advanoe 

(make-instance  'ayno-etate 

;name  'advance  {time-out  0.6 
! next-state  descont)) 

(setf  ready 

(make-inetance  'aaync-state 

mama  'ready  {command  'deploy-command 
mext-state  advance)) 

(send  return  : set-next-state  ready) 


(setf  atate  (cond  ((equal  a-etata-name  (sand  ready  :  state-name) ) 
ready) 

((equal  a-state-name  (send  advance  ; state-name)  ! 
advance) 

((equal  a-atate-name  (sona  descent  t state-name) ) 
descent) 

((equal,  a-state-name  (send  contact  :atate-name)  ) 
contact) 

((equal  a-state-name  (send  support  istate-name) ) 
support) 

((equal  a-state-name  (sjnd  lift  : state-name) ) 
lift) 

((equal  a-state-name  (send  return  : state-name) ) 
return) ) ) 


) 
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; (defmethod  (control-state-machine  jchango  jbafore) 
;  0 

/  (cond  ((typep  state  ' async-state) 

;  (if  (contact-sensor  : sensing) 

;  (setf  observation  ' contact-conf irm) 

i  (setf  observation  nil) ) 

/  ))) 


(defmethod  (oontrol-atate-mach.’ne  schange) 

0 

(cond  ( (typep  state  ' async-state) 

(if  (send  contact-sensor  i sensing) 

(setf  observation  ■' oontact-oonf irm) 

(setf  observation  nil) ) 

)) 

(oond  ((typep  state  'sync-state) 

(setf  state  (send  state  tchange))) 

(t  (setf  state  (send  state  tohangs  command  observation)))) 

;  ) 


•  t 

! (defmethod  (control-atate-machine  schange  tafter) 
t  0 

;  tend  command  to  executor  with  syno-atate-time 
(send  executor  s sand-command  (send  state  tstate-name) ) 
(if  (typep  state  'sync-state) 

(send  executor  sset-tima  (send  stats  tget-time)) 
(send  executor  i set-time  nil))} 


(defmethod  (control-atate-machine  isend-command) 
(a-oommand) 

(setf  command  a-command) ) 
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///  -*-  Mode : Common-Lisp;  Base: 10  -*- 

^********************************W**W****w*****W*********** 

;  display. globals 

;«**n**«*«*«w***www*w*  **«*«*  «»****  *«•****»  a**************** 

(defvar  eye-space  nil) 

(defvar  middle-of -screen  nil) 

(defvar  terrain- joystick) 

(defvar  graph-terrain) 

(defvar  graph-asv) 


;*****•*******«**********•**************#*******#***#**«*** 
i  display. library 

;******************************«********#****************** 


(defun  draw-to-earth  (a-point) 

(let  ( (draw-pt  (make-diaplayable 
middle-of-acraen 
(tranaform  eye-spaoe  a-point) ) ) ) 
(draw-to 

(liat  (truncate  (first  draw-pt)) 
(truncate  (second  draw-pt) ) ) 
•robot-window*) )  ) 


(defun  draw-to-*art.h-d  (a-point) 

(let  ((draw-pt  (make-diaplayable 
middle-of -screen 
(transform  eye-space  a-point)))) 
(draw-to-d 

(list  (trunoate  (first  draw-pt) ) 
(truncate  (second  draw-pt) ) ) 
•robot-window*) ) ) 


(defun  eraae-to-earth  (a-point) 

(let  ((draw-pt  (make-diaplayable 
middle-of -acreen 
(tranaform  eye-space  a-point)))) 
(erase-to 

(list  (truncate  (fir'st  draw-pt)) 
(truncate  (second  draw-pt) ) ) 
♦robot-window*) ) ) 


(defun  eye-trans  (eye-pt) 
i  eye-pt  (radius  alpha  beta) 
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I  eye  :■  orient *trans (0, 0, -r) *rot (x, -beta) *rot  (y, -alpha) »trana (-x, -y, -z) 
l  returns  aya-spaca 

l  library  :  idsnt,  transmat , rotate , matrixmult 
(let*  ((orient  (ident)) 

(rot  nil)  (trans  nil)  (eye  nil) 

(radius  (first  eye-pt))  (alpha  (second  aya-pt))  (beta  (third  aya-pt)) 
(cantar-of-intarast  (list  (/  (send  graph-terrain  tmax-x)  2) 

(/  (send  graph-terrain  imax-y)  2)  0))) 

(setf  (aref  orient  2  2)  -1.0) 

(setf  trans  (transmat  0  0  (-  radius))) 

(setf  eye  (matrixmult  orient  trans)) 

(setf  sot  (rotatemat  ' y-axis  (-  alpha))) 

(setf  eye  (matrixmult  eye  rot) ) 

(setf  sot  (rotatemat  'x-axis  (-  beta))) 

(setf  eye  (matrixmult  eye  rot) ) 

(setf  trans  (transmat  (-  (first  oenter-of-interest) ) 

(-  (second  eenter-of-intesest) ) 

(-  (third  oentsr-of-interest) )) ) 

(matrixmult  eye  trans))) 


(defun  make-displayable  (middle  pt) 

(let  ((soale  S000.0) 

(x  (first  pt))  (y  (second  pt) )  (s  (third  pt))) 
(list  <  +  (*  scale  (/  x  s))  (first  middle)) 

(+  (*  soale  (/  y  z) )  (second  middle) ))) ) 


(defun  move-to-earth  (a-point) 

(let  ((draur-pt  (make-displayable 
middle-of -screen 
(transform  eye-apaoe  a-point) ) ) ) 
(move-to 

(list  (truncate  (first  draw-pt) ) 
(truncate  (seoond  draw-pt) ) ) ) ) ) 


j**W#***(t*#*****#!H***##**##**#****##<***#**#******t* 

I 

i  joystiok  flavor  definition 

i 


(defflavor  joystiok ( (joy-x  0)  (joy-y  0)  (joy-r  0)) 
0 

tinitabla-instance- variables) 


(defmethod  (joystick  :get- joy-value) 

0 

(let*  ((key-value) 

(delta-x  0.2)  (delta-y  0.1)  (delta-r  0.01)) 

(setf  key-value  (my-read-ohar-no-hang) ) 

(cond  ((equal  key-value  '#\fi)  (setf  joy-x  (+  joy-x  delta-x))) 
((equal  key-value  '#\b)  (setf  joy-x  (-  joy-x  delta-x))) 


i 
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((equal  key-value  '#\r)  (aatf  joy-y  (-  joy-y  dalta-y))) 
((equal  kay-valua  '#\1)  (aatf  joy-y  (+  joy-y  dalta-y))) 

( (eq ual  fcey-value  '#\-)  (aatf  joy-e  (-  joy-r  dalta-r))) 

( (equal  kay-valua  '#\-)  (aatf  joy-r  (+  joy-r  dalta-r)))) 

(cond  ((>-  joy-x  2)  (aatf  joy-x  2)) 

( <<«  joy-x  -2)  (aatf  joy-x  -2))) 

(cond  ( (>■  joy-y  1)  (aatf  joy-y  1)) 

! (<■  joy-y  -1)  (aatf  joy-y  -1))) 

(cond  ( (>“  joy-r  0,1)  (aatf  joy-r  0.1)) 

( (<■  joy-r  -0.1)  (aatf  joy-r  -0.1))) 

(cond  ((aqual  kay-valua  '#\q)  (aatf  joy-x  0) 

(aatf  joy-y  0)  (aatf  joy-r  0))) 

(liat  joy-x  joy-y  joy-r  (aqual  kay-valua  '#\x))>) 


.(dafmathod  (joyatlek  traaat) 

0 

(aatf  joy-x  0) 

(aatf  joy-y  0) 

(aatf  joy-r  0) ) 


(aatf  tarrain- joystick  (maka-inatanca  ' joyatick) ) 


^***************«**ili**ilMlr**W**************************«*** 

I 

i  tarrain  flavor  dafinitlon 
/ 


(daf flavor  tarrain ( (tarrain-data  (maka-array  '(49  49)  :initial-alamant  0)) 
tarrain-height-array  tarrain-haight-liat  joystick 
(ouraor-xl  (oursor-y)  (max-x)  (max-y) 

(radius  300)  (alpha  0)  (bata  0)) 

0 

: initabla-instanoa-variablas 
igattabla-instanaa-variablaa) 


(dafmathod  (tarrain  : areata) 

0 

(aand  aalf  sinitti) 

(sand  aalf  modify) 

(my-print  "Now  usa  joystick  to  control  tha  robot.'')) 


(dafmathod  (tarrain  :kill) 

<> 

( kill-robot -terrain-windows) 
(restore-lisp-listener) ) 


(dafmathod  (tarrain  :initti) 

0 

;  globals  :  middle-of-screan,  aye-spaca 
(move-and-shape-lisp-listener) 
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(lot  ((arrsy-dims  (array-dimanaiona  tarrain-data) ) ) 

(satf  radius  S00  alpha  0  bata  0) 

(satf  max-x  (first  array-dims)) 

(satf  max-y  (sacond  array-dims) ) 

(satf  cursor-x  (floor  (/  max-x  2))) 

(aatf  euraor-y  (floor  (/  max-y  2)))) 

(aatf  tarrain-haight-array  (maka-array  (4-  max-x  1))) 
(maka-robot- window) 

(aatf  middla-of-aoraan 

(middla-of-robot-window) ) 

(aatf  aya-apaoa  (aya-trana  (list  500  00))) 

(aand  naif  i input -tarrain-paramatara) 

(my-print  "Plaasa  uaa  joystick  to  transform  tha  tarrain.") 
•  (my-print  "Wait.”) 

(maka-viaibla) 

(aand  aalf  taraaa-obstaolaa) 

(my-print  "Now  you  oan  tranalata  tha  tarrain.")) 


(dafmathod  (tarrain  modify) 

O 

;  axtarnal  :  aya-apaoa 
(do  ( (dalta  0.0001) 

( joyatiok-valua  nil) 

(and-flag  nil) ) 

(and-flag  (my-print  "Wait.") 

(aand  joystick  iraaat) 

(aand  aalf  laava-tarrain  aya-apaoa) 

(aand  aalf  idraw-obataolaa) ) 

(maka-viaibla) 

(aatf  joyatiok-valua  (aand  joystick  igat-joy-valua) ) 

(lat  ((x  (first  joyatiok-valua)) 

(y  (aaoond  joyatiok-valua)) 

(r  (third  joyatiok-valua) ) 

(fira  (fourth  joyatiok-valua))) 

(aand  salf  lerasa-tarrain) 

(cond 

(fira  (oond  (  (us«r-ok) 

(oond  ( (usar-sava) 

(aand  aalf  :sava’>t«rrain-to-disk  (usar-fila-nama) ) ) ) 
(aatf  and-flag  t ) ) 

(t 

(aand  joystick  sraaat) 

(aatf  joyatiok-valua  (aand  joyotiok  igat-joy-valua))))) 

((>  x  dalta)  (satf  alpha  <4-  alpha  0.05))) 

( (<  x  (-  dalta))  (aatf  alpha  (-  alpha  0.05))) 

((>  y  dalta)  (aatf  bata  (4-  bata  0.05))) 

( (<  y  (-  dalta))  (satf  bata  (-  bata  0,05))) 

((>  r  dalta)  (satf  radiua  (+  radius  10))) 

( (<  r  (-  dalta))  (aatf  radiua  (-  radius  10)))) 

(aatf  aya-spaca  (aya-trana  (list  radiua  alpha  beta) ) ) 

(sand  salf  ;draw-tarrainl  aya-spaca)))) 


(dafmathod  (tarrain  : in-sida-of-whola-tarrain) 

(a-poa) 

(lat  ( (dimanaion-tarrain  (array-dimansions  tarrain-data) ) 
(i-x  (floor  (first  «.  poa))) 

(i-y  /floor  (second  a-poa)))) 

(cond  ( (<  i-x  0)  nil) 
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( <<  i-y  0)  nil) 

((>  i-x  (••  (first  dimension-terrain)  1))  nil) 
((>  i-y  (-  (second  dimension-terrain)  1))  nil) 
(T)  ) 


(dofmethod  (terrain  :permitted-cell) 

(terrain-pos) 

( 1st  < (i-x  (floor  (first  terrain-pos) ) )  t  find  terrain  index 
(i-y  (floor  (sooond  tesrsin-pos) )) ) 

(if  (sand  self  i in-side-of-whole-terrsin  terrsin-pos) 

(if  (equal  (aref  terrain-data  i-x  i-y)  0)  i  permitted 
t 

nil)))) 


(dofmethod  (terrain  i torts in-point) 
(s-poe-wrt-esrth) 

(lot*  ((x  (first  s-poo-wrt-esrth) ) 

(y  (sooond  s-poe-wrt-esrth) ) 

(*  (send  solf  jget-height  (list  x  y)))) 
(list  x  y  s))) 


(dofmothod  (tor coin  i get -height) 

(s-poo-wrt-esrth) 

i  rsngo  0  •<  x  <»  (first  dimension-terrain-height) ,  (0  <  x  <39) 
i  o  -<  y  <-  (sooond  dimonsion-torroin) . 

(lot*  ((dimonsion-torrsin-hoight  (srrsy-dimonsions  torrsin-hoight-srrsy) ) 
(x-min  0)  (x-msx  (first  dimonsion-torrsin-hoight) ) 

<x  (first  s-pos-wrt-osrth) ) ) 

(if  (or  (<  x  x-min)  (>  x  x-msx)) 

-1000 

(lot*  ((i-x  (floor  x) )  ;  got  terrain  x-indox 

(xl  (if  «  (-  x  i-x)  0.5)  (-  i-x  1)  i-x)) 

<x2  (if  «  <-  x  i-x)  0.5)  i-x  (+  i-x  1))) 

(xl  (if  (<  xl  x-min)  0  xl) ) 

(x2  (if  (>-  x2  x-msx)  (-  x-msx  1)  x2) ) 

(si  (aref  torrsin-hsight-srrsy  xl)) 

(n2  (srsf  torrsin-hoight-srrsy  x2)) 

(slope  (-  c2  cl) ) 

(dol-x  (-  x  xl) ) ) 

(+  s'i  (*  slops  dol-x)))))) 


(sotf  grspli-torrsin  (make-instence  'terrain 

ijoystiok  terrain- joystick) ) 


;  terrain ,  input-t.errain-parsmetors 
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(dafmathod  (tarrain  : inputt'-tarrain-paramatara) 

0 

(initialiia-manu-variables) 

(cond  ((aatf  *old-tarrain-£.Ua~nama*  (gat-old-tarrain-fila-nama) ) 

(sand  graph-tarrain  i raad-tarrain-from-diak  *old-tarrain-£ ila-nama*) ) 
(t 

(aand  aalf  igat-naw-tarrain) ) ) ) 


(dafmathod  (tarrain  tgat-naw-ttrrain) 

0 

(•and  aalf  igat-naw-tarrain-haight) 
(aand  aalf  :draw-tarrain  aya-apaoa) 
(aand  salf  laat-naw-terrain-obataola)) 
(sand  naif  :aat-n«w-ditch) ) 


(dafmathod  (tarsaln  tgat-naw-tarrain-huight) 

0 

(1st  ((alopa-typa  (gat-tarrain-alopa-typa) ) 

(angla  nil)  (data  nil)) 

(aond  ((aqual  alopa-typa  ' eingla-angla) 

(aatf  angle  (gat-taerain-alopa-angla) ) ) 

((aqual  alopa-typa  'manual) 

(aatf  data  (gat<tae«ain-alopa-data) ) ) ) 

(my-print  "Wait.") 

(aand  aalf  t raad-tarrain-halght  alopa-typa  angla  data))) 


(dafmathod  (toKcain  laat-naw-tarrain-obataolaa) 

0 

(lat  ( (tarrain-typa  (gat-tarrain-obataela-typa) ) 

(valuaa  nil) 

(obatacla-ratio  nil)  (rundom-saad  nil)) 

(aond  ((aqual  tarrain-typa  'random) 

(aatf  valuaa  (gat-tareain-randum-data) ) 

(aatf  obataola-ratio  (firat  valuaa)) 

(aatf  random-saad  (aacond  valuaa)) 

(my-print  "Wait.") 

(aand  aalf  : candom-tarrain  obataola-ratio  random-aaad) ) 
(t 

(aand  aalf  sdiaplay-ouraor) ) ) ) ) 


(dafmathod  (tarrain  i aat-naw-ditoh) 

0 

(lat  ( (di.toh-typa  (gat-ditch-typa) ) 

(width".\ooation  nil)) 

(cond  ((equal  ditch-typa  'add-ditch) 

(aatf  width -location  (get-ditch-width-location) ) 
(my-print  "Wait.") 

(aand  aalf  sadd-ditch  (firat  width-location) 
(aacond  width-locution)) 

(aand  aalf  :draw-obataclaa) 

(maka-viaible) ) 
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(t  nil)))) 


;********»***)»****************** ***********  **************** 
/  terrain . diaplay-torrain 

,*****»****»«***W********1.*****«***. ***»»**,,****, ***,*.*** 


(r  nil)  (fir#  nil) 


(dafmathod  (taerain  sdiaplay-ouraor) 

0 

(•and  aalf  imake-all-permittad) 

(do  ((joy-data  nil)  (x  nil)  (y  nil) 

(exit-flag  nil) ) 

(axit-flag  (land  aalf  saraaa-ouraor  (liat  curaor-x  ouraor- 
,  (make-viaibla) 

(aatf  joy-data  (aand  joyatick  igat- joy-valua) ) 

x  (-  (aaoond  joy-data)))  (aatf  y  (firat  joy-data)) 
r  (third  joy-data))  (aatf  fira  (fourth  joy-data)) 
saraaa-ouraor  (liat  ouraor-x  ouraor-y) ) 


•y>> 


(aatf 

(aatf 


(aand  aalf 
(oond 

(fira  (aatf  axit-flag  t) ) 

((>  x  0)  (aatf  ouraor-x  (+  ouraor-x 


(if  (>  ouraor-x  max-x) 

(aatf  ouraor-x  max-x) ) ) 
(if  (<  ouraor-x  0) 

(aatf  curaor-x  0) ) ) 

(if  (>  ouraor-y  max-y) 

(aatf  euraor-y  max-y) ) ) 
(if  (<  ouraor-y  0) 

(aatf  ouraor-y  0))) 

0)  (aatf  (araf  tacrain-ctata  curaor-x  ouraor-y)  D) 

0)  ( oat f  (araf  tarrain-data  ouraor-x  ouraor-y)  1))) 

(•and  aalf  idraw-curaor  (liat  ouraor-x  ouraor-y)) 

(aand  aalf  sdraw-obataolea) 

(aand  joyatiok  sraaat))) 


(«  x  0) 
<<>  y  0) 
<«  y  0) 


<« 

((> 


(aatf  ouraor-x 
(aatf  ouraor-y 
(aatf  ouraor-y 


1) ) 

(-  ouraor-x  1)) 
<4  ouraor-y  1)) 
(-  ouvaor-y  1)) 


(dafmathod  (tarrain  sdraw-tarrain) 

(aya-apaoa) 

i  axtarnal  function i  \diaplay , library\move-to-#arthr  draw-to-aarth 
(dotimaa  (x  (+  max-x  1)) 

(mova-to-aarth  (liat  x  0  (araf  tarrain-haight-array  x) ) ) 
(draw-to-aarth  (liat  x  max-x  (araf  tarrain-haight-array  x) ) ) ) 
(dotimaa  (y  (+  max-y  1)) 

(mova-to-earth  (liat  0  y  0)) 

(dotimaa  (x  (+  max-x  1)) 

(draw-to-aarth  (liat  x  y  (araf  tarrain-haight-array  x)))))) 


(defmethod  (tarrain  jdraw-tarrainl) 
iaya-apaaa) 

t  axtarnal  function:  \diaplay, library \mova-to-earth,  draw-to-aarth 
(do  ( (xa  (liat  0  max-x)  (odr  xa) ) 

(x  nil) ) 

( (null  xs)  ) 

(aatf  x  (oar  xa) ) 

(mova-to-earth  (liat;  x  0  (araf  tarrain-haight-array  x) ) ) 
(drow-to-eurth  (liat;  x  max-x  (araf  tairain-height-array  x) )  ) ) 
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(do  ( (ya  (liat  0  max-y)  (cdr  ya) ) 

<y  nil) ) 

( (null  ys) ) 

(satf  y  (car  ya) ) 

(move-to-earth  (liat  0,  y  0)) 

(dotimas  <x  (+  max-x  1)) 

(draw-to-earth  (liat  x  y  (ara£  terrain-height-array  x) ) ) > ) ) 


(dafmathod  (tarrain  leraae-obatacles) 

O 

l  axtarnala  i  tarrain 

l  axtarnal  function:  \di#play .librarySmova-to-aarth,  draw-to-earth 
(dotimaa  (i  (firat  (array-dimensions  tarrain-data) ) ) 

(dotimas  (j  (aaoond  (array-dimanaiona  tarrain-data))) 

(oond  ((equal  1  (araf  tarrain-data  i  j)) 

(move-to-earth  (liat  i  j)) 

(araaa-to-aarth  (liat  (til)  (t  j  1))) 

(mova-to-aarth  (liat  (+  i  1)  j)5 
(araaa-to-aarth  (liat  i  <  +  j  1) ))))))) 


(dafmathod  (tarrain  taraaa-tarrain) 
0 

(olaar-robot-window) ) 


(dafmathod  (tarrain  imaka-all-parmittad) 
0 

(dotimaa  (i  max-x) 

(dotimaa  (j  max-y) 

(aatf  (araf  tarrain-data  i  j)  0)))) 


(dafmathod  (tarrain  : read-tarrain-height) 

(tarrain-alopa-typa  terrain-slope-angle  tarrain-alope-data) 
(cond  ( (aqual  tarrain-alopa-typa  'default) 

(aatf  tarrain-haight-list  '((19  0)  (25  1)  (35  1.5)))) 

((equal  tarrain-alopa-typa  'single-angle) 

(let*  ((angle  (*  pi  (/  terrain-alopa-angle  180))) 

(max  (*  20  (tan  angle)))) 

(aatf  tarrain-haight-list 
(liat  ' (20  0) 

(liat  40  max) 

)))) 

(t  (aatf  tarrain-haight-liat  terrain-slope-data) ) ) 

(let*  (  (xl  0)  (zl  0)  (a-pair)  <zz  0) 

(x2  (firat  (car  tarrain-haight-liat))) 

(c2  (aaoond  (car  tarrain-haight-liat))) 

(alope  (/  (-  z2  zl)  (-  x2  xl) ) )  ) 

(aatf  tarrain-haight-list  (cdr  tarrain-haight-list)) 

(dotimas  (i  (+  max-x1  1)) 

(aatf  zz  (+  (*  slope  (-  i  xl))  zl) ) 

(con'd  t  (aqual  x2  i) 

(aatf  xl  x2) 

(cond  ( (aatf  a-pair  (car  tarrain-haight-liat) ) 

(satf  tarrain-haight-list  (cdr  terrain-height-list)) 
(snt£  x2  (first  a-pair)) 

(satf  z2  (second  a-pair) ) 

(aatf  zl  zz) 
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(setf  slope  (/  (-  z2  *1)  (-  x2  xl)))) 
(T  (setf  slope  0)  (setf  zl  zz))))) 
(setf  (aref  terrain-height-array  i)  zz)))) 


(defmethod  (terrain  : save -terrain) 
(eye-space) 

(send  self  idrawobstaoles) 

(send  self  idraw-terrain  eye-space) 
(save-terrain-to-fcerrain-buffer) ) 


(defmethod  (terrain  : save-terroin-to-disk) 

(file-name) 

(with-open-iile 

(out-file 

(merge-pathnames  file-name  "robot ikwak, robot. terrain-data  data")  :direotion  :outp 

) 

(setf  *print-array*  t) 

(print  terrain-data  out-file) 

(print  terrain-height-array  out-file) 

(print  radius  out-file) 

(prim,  alpha  out-file) 

(print  beta  out-file) 

(setf  *print-array*  nil))) 


(defmethod  (terrain  :read-terrain-from-disk) 

(file-name) 

(with-open-file 

(input-file 

(merge-pathnames  file-name  "robotskwak. robot. terrain-data/")  [direction  : input ) 
(setf  *print-array*  t) 

(setf  terrain-data  (read  input-file)) 

(setf  terrain-haight-array  (read  input-file) ) 

(setf  radius  (read  input-file) ) 

(setf  alpha  (read  input-file)) 

(setf  beta  (read  input-file)) 

(setf  *print-array*  nil))) 


j***********************************«********************** 

i  terrain. display-cursor 

^ ********************** **********it*t*********************** 


(defmethod  (terrain  idraw-oursor) 
(  position  ) 

(let*  ((x  (first  positioh)) 


(y  . 

(second  position) ) 

(pi 

(list 

(+  x  0.2) 

(  + 

y 

0.2) 

0)  ) 

<P2 

(list 

(+  x  0.8) 

(  + 

y 

0.2) 

0)  ) 

(p3 

(list 

(+  x  0.8) 

(  + 

y 

0.8) 

0)! 

(p4 

(list 

(+  x  0.2) 

(  + 

y 

0.8) 

0)  ) 

(points  (list  p2  p3 

p4 

pi) ) ) 

(meve-to-earth  pi) 

(do  ((points  points  (edr  points))) 
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((null  points)  'done-draw-cursor) 
(draw-to-earth  (oar  points))))) 


(defmethod  (terrain  (draw-obstacles) 

0 

(dotimes  <i  max-x) 

(dotimes  (j  max-y) 

(oond  ((equal  1  (aref  terrain-data  i  j) ) 

<mova-to-earth 

(list  i  j  (aref  terrain-height-array  i ) ) ) 

(draw-to-earth 

(list  <+  i  1)  (+  j  1)  (aref  terrain-height-array  (+  i  1)))) 
(move-to-earth 

(list  (+  i  1)  j  (aref  terrain-height-array  (+  i  1)))) 
(draw-to-earth 

(list  i  (+  j  1)  (aref  terrain-height-array  i) )))))) ) 


(defmethod  (terrain  terase-cursor) 
(  position  ) 

(let*  ( (x  (first  position)) 


<y 

(second  position)) 

<pi 

(list 

<+  x  0.2) 

<+  y 

0.2) 

0)) 

<P2 

(list 

<4-  x  0.8) 

<+  y 

0.2) 

0), 

<P3 

(list 

(+  x  0.8) 

(+  y 

0.8) 

0)) 

(p4 

(list 

(+  x  0.2) 

<+  y 

0.8) 

0)> 

(points  (list  p2  p3  p4  pi) ) ) 

(move-to-earth  pi) 

(do  ( (ooints  points  (odr  points) ) } 

((null  points)  'done-evise-curser) 
(erase-to-earth  (oar  points))))) 


(defmethod  (terrain  : random-terrain) 

(obstacle-ratio  random-seed) 

(let  ((a  43411)  (b  17)  (c  640001) (percent  nil)  (seed  nil)  (x  nil)) 
(setf  percent  obstacle-ratio) 

(setf  seed  random-seed) 

(setf  x  seed) 

(dotimes  (i  max-x) 

(dotimes  (j  max-y) 

(if  (<  (/  (setf  x  (mod  (+  (*  a  x)  b)  c))  c)  (/  percent  100)) 
(setf  (aref  terrain-data  i  j)  1))))) 

(send  self  (draw-obstacles) ) 


(defmethod  (terrain  : add-ditch) 

(width  location) 

(aotimes  (i  width) 

(dotimes  (j  max-y)  ’ 

(setf  .(aref  terrain-data  (+  i  location)  j)  1)))) 


.**************>.**;t*****************************4********** 


graph-vehicle  flavor  definition 


diaplay-t2 . liap  Wad  Nov  28  10;  10: 22  1990  11 

,***************  ***************** a*-********** ************* 

(def flavor  graph-vehicle ( (vchicle-pointa  (make-.'irray  28)) 

(body-points  (mako-array  10) ) 
(polygons  (make-array  13) ) 
(numpolya  nil) 

(verticea  (make-array  100))) 

0 

tinitable-instance-variables) 


(defmethod  (graph-vehicle  sinit-data) 

O 

(aend  self  iread-vehicle-data) )  ; 


read  data  from  diak 


(defmethod  (graph-vehicle  idiaplay) 

(a-H  £oot-£oaitiona) 

( clear- 'robet-windpw) 

(aend  eel*  :body-panto-wrt~earth  a-H  f  oof, -positions) 
(aend  aelf  :draw-vehicle  vehicle-poir.ta) 
(copy-terrain-to-rdbot-window) 

(make-visible) ) 


(defmethod  (graph-vehicle  sread-vehicle-datr.) 

0 

/  global  variables  ;  vehiole-pointa,  polygona,  numpolya,  verticea 
;  format  of  file  i  num-vf -points  num-of-polygons 

;  (  num  a-vehiole-point)  .... 

;  (  num-of-verticea  verticee-number-of-a-polygon) . . . 

(let*  ((vehicle-file  (open  "exp3 :kwak, robot /vehicle. date" )  ' 

(numpca  (read  vehicle-file)) 

(numvtcee  0)  <a-polygon  nil) ) 

(aetf  numpolya  (read  vehicle-file)) 

(dotimea  (i  numpta) 

(aetf  (aref  vehicle-points  i)  (cdr  (read  vehicle-file)))) 

(dotimea  (i  10) 

(aetf  (aref  body-points  1)  (aref  vehicle-points  i))) 

(dotimea  (i  numpolya) 

(aetf  a-polygon  (reed  vehicle-file) ) 

(aetf  (eref  polygona  i)  (liat  numvtcea  (car  e-polygon)/) 

(do  !  (a-polygon-verticea  (cdr  a-polygon)  (cdr  a-polygon-verticea) ) 

(j  0  (+  j  1))) 

((null  a-polygon-verticea)) 

(aetf  (aref  vertioes  (+  numvtcea  j)) 

(-  (first  a-polygon-verticea)  1>>> 

(aetf  numvtcea  <+  numvtcea  (car  a-polygon)))) 

(close  vehicle-file))) 


(sat*"  graph-aav  (make- instance  'graph-vehicle)) 


;  graph-vehicle, display 
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I 

j  *************************  ********************************* 


(dafmethod  (graph-vahicla  : body-pent «-wrt -as rth) 

<  a-H  foot-positions  ) 

;  library  :  transform 

(lat  ((al  0.6616)  (a2  0.945)  03  3.108)  (10.6133)  (m  1.0467) 
I  (hipx-list  '(5.1667  5.1667  0.0  0.0  -4.9167  -4.9167)) 
(hipx-list  '  (6.0  6.0  0.0  0.0  -6.0  -6.0)5 
(hipy-liat  '  (1.62  -1.62  1.62  -1.62  1.62  -1.62)) 

(aignl-list.  ' <l  -l  l  -l  l  -i)) 

(aign2-list  '(1  1  .■!, ;1  -1  -1))) 

(aand  aalf  itxansform-body-pointa  a-H  faody-pointb) 

(do  ( (positions  foot-positidnr  («dr  positions) ) 

(hipx-list  hipx-list  (cdr  hipx-list) ) 

(hipy-list  hipy-list  (cdr  hipy-list)) 

(signl-liot  aignl-list  (cdr  signl-liat) ) 

(sign2-lUt  aigni-liat  (odr  aign2-liatj) 

(i  0  (+  i  1))) 

((hull  positions)  nil) 

(lat*  ( (foot-poa  (oar  positions) ) 

(hipx  (oar  hipx-list))  ,'hipy  (oar  hipy-liat)) 

(aignl  (oar  signl-liat) )  (aign2  (oar  sign2-list)) 

(px  (-  (first  foot-pos)  hipx)) 

(py  (-  (second  foot-pos)  hipy) ) 

(ps  (third  foot-pos)) 

(thata  (vahiola-thata  py  ps  m  signl) ) 

(dm  (vahiela-dm  px  sign2) ) 

(dl  (vahiola-dl  py  ps  m  1)) 

(top-poa  nil)  (knaa-^oa  nil)) 

(satf  top-poa 

(transform  a-H 

(vahiola-top-pos  hipx  hipy  m  1  dl  thata  aignl))) 
(satf  knaa-poa 

(transform  s-H 

(vahiola-knaa-pos  hipx  hipy  m  1  si  o2  s3 
dl  dm  thata  aignl  sign2))) 

(satf  foot-pos  (transform  a-H  foot-pos)) 

(satf  (araf  vahicla-points  (t  10  <*  3  i) ) ) 
top-poa) 

(satf  (araf  vahiola-points  (+  11  (*  3  i) ) ) 
knae-pos) 

(aatf  (araf  vahiola-points  <+  12  <*  3  i) ) ) 
foot-pos) ) ) ) ) 


(dafmathod  (graph-vahicla  idrew-vahicla) 

(  vahiola-points  ) 

;  global  variables  :  polygons,  numpolyj,  vartioas 
(dotimes  (i  numpolys) 

(lat  ((start  (first  (sraf  polygons  i)J) 
(num-varticas  (saoond  (araf  polygons  i) ) ) ) 
(mova-to-earth  (araf  vahicle-points 
(sraf  vertibea  start))) 

(dotimas  (j  num-vertioas) 

(draw-to-aarth-d  (araf  vahiola-points 
(sraf  vertices  (+  start  j))))) 

))) 
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j**************w*********  A  *******■*******%****#****  i’  ******** 

;  !- 

/  graph-vehiela .display .body-rpanto-wrt -earth 
i 


(dafmathod  (graph-vehicle  ttra.hsforifi-boJy-poifita) . 

(s-H  body-pointa!  ■.  ■ 

;  global*  :  vehicie-poir.ts  ' 

;  library  :  transform 
(dotirraa  (i  10) 

(setf  (araf  vehicle-points  1} 

(transform  'a-H  (Sra£  bodypoihta  i))))) 


(defur  vahioia-dl  (py  |p»  m  1) 

(V  (-  (aqrt  (+  (*  py  py)  f*  pi  pi)  (-  (*  mm))))  1) 
0) 


(dafun  vehial«#-dm  <px  aign2) 
(*  aign2  (/  px  5))) 


(dafun  vehicle-knee-poa  (hipx  hipy  m  1  al  a2  a3 
dl  or.  that*  signl  sign2) 

(let*  ( (numar  <+  (*  al  al)  <-  ("  s2  s2))  (*  eU  di)  (*  dm  dm))) 
Idanom  (*  2  al  (aqrt  (+  ( *  dl  dl)  (*  dm  dm))))) 

(bata  (aooa  (/  numar  danom) ) ) 

(alpha  (-  (/  pi  2)  tatan  dm  dl)  bata)) 

(aina  (tin  alpha))  (cosa  (cos  alpha)) 

(sint  (ain  thata))  (cost  (con  thata)) 

(tamp  (■  (*  s3  aina)  <-  dl  1))) 

(xk  <*  (*  aign2  s3  oosm  hipx) ) 

(yk  (+  (*  signl  (+  (*  tamp  flint)  (*  m  cost)))  hipy)) 

(tk  (-  (+  (*  tamp  cost'  <*  m  sint))))) 

(list  xk  yk  ik) ) ) 


(dafun  vahiola-thata  (py  pt  m  signl) 

(let*  ( (anglal  (atan  (*  signl  py)  <*  -1  pt) ) ) 
(angle2  (atan  m  (aqrt  (+  {*  py  py) 

<*  pi  pi) 

<-  (*  a  m))))))) 

(-  anglel  angla2))) 


(dafun  vahicla-top-pos  (hipx  hipy  m  1  dl  thata  aig.il) 
(lat*  <  (xt  hipx) 

(1-dl  (-  1  dl) ) 

(aina  (sin  thata) ) 

(cosa  (cos  theta) ) 

(yt  (+  (*  signl  (  +  (*  m  cosa)  (*  1-dl  sina) ) )  hipy)) 
(it  (-  (*  m  sina)  (*  1-dl  cosa)))) 

(list  xt  yt  it) ) ) 


1 


ditch-robot-: .lisp  Thu  Nov  29  11:28:36  1980 

ill  Mode: Common-Lisp:  Base: 10  -*- 

;************************************»**********************»******* 

I 

i  ditch-robot  definition 
/ 

J*********** **************************************** **************** 

(def flavor  ditch-robot  0 

(teat-overlap-robet) 

) 


(defmethod  (ditoh-robot  linitti) 

0 

(send  greph-asv  tinit-deta) 

(eetf  viaion-ayafcom  (make-instance  'ditoh-vision-aystem  towner  self)) 
(send  vision-system  tinitti) 

(sotf  joystick  (make-inatanae  ' joystick) ) 

(send  joystick  t reset) 

(empty-queue  lift-queue) 

(setf  lift-flag  t) 

(let  ((H)) 

(setf  body  (make-instance  'stop-body  townar  self)) 

(setf  H  (send  body  iinitti)) 

(setf  legs  (list 

(make-instanee  'test-overlap-leg  mama  'legl  towner  self) 
(make-instanoe  'test-overlap-leg  tname  'leg2  towner  self) 
(make-instanoe  'test-overlap-leg  tname  'leg3  towner  self.) 
(mako-instande  'test-overlap^leg  tname  'leg4  towner  self) 
(make-instanoe  'test-overlap-leg  tname  'leg5  towner  self) 
(make-instanoe  'test-overlap-leg  tname  'leg6  towner  self) 
)) 

(mapoar  #' (lambda  (a-leg)  (send  a-leg  tinitti  K))  legs)) 

> 


(defmethod  (ditch-robot  iat-stability-limit) 
0 

(nut  (send  self  tatable))) 


(defmethod  (ditch-robot  : stop-motion) 
0 

(sctnd  body  t  at  op-body-motion) ) 


(defmethod  (ditoh-robot  : resume-motion) 

0 

(send  body  : restore-body-motion) 
t) 


;******************************************************* 


(defun  at_stability_limit  () 

(send  asv  sat-stability-limit) ) 
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(dsfun  stop_»otion  () 

(ssnd  «sv  i stop-motion) ) 


(defiun  r**um*_mot ion  () 

(esnd  asv  irssums-motion) ) 


ditoh-viaion-t .lisp  Thu  Nov  29  11; 28: 50  19  90  X 

III  ModatCommon-Liapi  Base:  1C 

J**********************#*lt**lHr**lt«1*****W«#l»»****«*i*l«W1»»l*****r**#***l** 

; 

i  ditch-vision-system  definition 
) 

;****»Wl***»|lr<r***'***lH***lHMf*****#**#*i*l*#****#*lM(lHr******************* 


(daf flavor  ditch-viaion-ayatem  () 
(vision-ayatem) 

) 


(dafmathod  (ditoh-vision-eystem  :on-ditoh-area) 
(body-HIO) 

(let  ((x  (araf  body-HIO  03))) 

(oond  ((and  (»  x  (-  21  7)) 

(<■  x  (+  21  *ditch-width*) ) ) 
t) 

(t  nil)))) 


(aatf  *ditch-width*  6) 


•a 


I 
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it i  Mod# :Common-Liap;  Base:  10  -*- 


1 


t 

l  executor  flavor  daflnltion 
; 


(dofflavor  oxecutor 

(lag-pos-wrt-body  deaired-foothold-pos-wrt-earth 

tima  command  owner  aanaor  (lift-height  1.4) 
<T1  0.6)  ( T 2  1.0)  (T3  0,4)  (T4  0.6) 
(planned-oontact-tima  0.4)  aalf-tima 
(aampling-tima  0.1)  xeady-poa 
HI  inv-Hl  body-trana-satal  bodyrotata-ratal) 
O 

linitable-instance-variablea) 


(dafmathod  (axacubor  : set-daaixed-poa) 

(a-poa) 

(satf  deaired-foothold-pos-wrt-earth  a-poa)) 


(dafmathod  (axaoutor  igat-dasixad-poa) 


0 

daairad-foothold-poa-wrt-aarth) 


(dafmathod  (axacutor  laand-command) 
(a-oommand) 

(satf  command  a-oommand)) 


(dafmathod  (axacutor  :aat-tlma) 
(a-tlma) 

(satf  tima  a-tima)) 


(dafmathod  (axaoutor  : lag-pos-wrt-body) 

0 

leg-poa-wrt-body) 


(dafmathod  (axaoutor  tmova) 

(H  inv-H  toody-trans-rata  body-rotata-rata) 
(aatf  HI  H) 

(aatf  inv-Hl  inv-H)  1 

(aatf  bodyrtrans-ratal  body-trins-rats) 

(aatf  body-rotata-ratel  body-rotata-rate) 

(cond  ((equal  command  '  raady) 

(aand  aalf  smova-in-raady) ) 

((aqual  command  'advance) 

(aand  aalf  :mcva-in-advance) ) 

((equal  command  'daacent) 

(aand  aalf  :move-in-daacent) ) 
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((equal  command  'contact) 

(send  self  imova-in-contaot) ) 
( (aqual  command  'support) 

(sand  aalf  imove-in -support) ) 
((equal  command  'lift) 

(sand  self  :mova-in-lift) ) 
((equal  command  'return) 

(send  self  imove-ii;--*turn) ) 

) 

) 


(dafmethod  (executor  smove-in-oontaot) 

0 

(let  ( (leg-vclocity-wrt-body  (send  self  :£ind-vsi.oeity-wrt-body) ),) 
(setf  leg-pca-wrt-body 

(vectedd  (nagvact  sampling-time  l,eg-velr>city-wrt-isody) 
leg-pos^wtt-body) ) ) ) 


(defmethod  (executor  ifind-velooity-wrt-body) 

O' 

I  returns  foot-yelooity-wrt-body  ,  ,-i 

i  velocity  ■  -  (  body-trans-rate  +  body-rotaterrate  X  ley-poa  ) 

I  globala  v  :  body-trans-ratal,  body-rotate-ratal 
l  lib  i  veotaub,  veotadd,  ororsprod 
(veotsub  '(0  0  0)' 

(ver.tadd  body-trane-ratcl 

(oroa sprv'd  body-rotate-ratel  leg-pos-wrt-body) ) ) ) 


(dafmethod  (executor  imove-in-edvanoe) 

O 

(let  ( (deeired-poa  (aend  self  :deeired-edvanoe-poa-wrt-borty) ) 
(dt  (-  T1  time))) 

(eend  eelf  move-del  deaired-poa  leg-poe-vrt-body  dt) ) 

(setf  self-time  0.0)) 


(defmethod  (executor  ideaired-advanoa-pos-wrt-body) 

0 

;  a-pos  is  dasired-stepping-pos-wrt-earth 
;  returns  deeired-poa-wrt-body  in  deploy  state 
/  global  variable  i  HI,  iuv-Hl 

i  global  function  j  to-earth-transform,  tn-body-trsnsform,  find-terrain-hegiht 
(let*  ( (desired-pos-wrt-earth  desired-foothold-pos-wrt-earth) 

(terrain-height  (third  (send  owner  iterrain-poiut  desired-pos-wrt-earth))) 
(desired-pos-height-wrt-earth  (+  terrain-height  lift-height)) 
(poe-wrt-aarth  (list  (first  desired-pos-wrt-earth) 

(second  desired-pos-wrt-earth) 
deaired-poe-height-wrt-earth) ) ) 

(to-body-transf orm  inv-Hl  pos-wrt-aarth) ) ) 


(dafmathod  (executor  :move-in-daacent) 

0 

/  global  function  :  to-body-transform 
;  global  variables  :  inv-Hl 

(let  ( (dt  (-  planned-contact-time  self-time) ) ) 
(if  (<  dt  0.05) 
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(aetf  leg-poa-wrt-body  (to-body -transform 

inv-Hl  dasired-foothold-pos-wrt-earth) ) 

(send  eel*  imove-deL 

(to-bodjf-tranaform  inv-Hl  desired-foothold-poa-wtt-eerth) 
leg-poa-wrt-body  dt) ) ) 

i  > 


; (defmethod  (executor  tmove-in-deaoent  i after) 

/  0 

(4etf  aelf-tiire  (+  aelf-time  aempling-time) ) ) 


(defmethod  (executor  move-del) 

(deaired-poa  preaent-poa  dt) 
i  aet  new  leg-poe  depending  on  the  arguments 
l  lib  i  veotadd,  magveot 
'if  «  dt  0.05) 

(aetf  leg-poe-wrt-body  deaired-poa) 

(let*  ( (inv-time-dif f  </  1  dt)) 

(del  (veoteub  deaired-poa  preaent-poa)) 

(velocity  (magvect  inv-time-diff  del))) 

(setf  leg-poa-wrt-body 

(veotadd  preaent-poa  (megvect  aampling-time  veloaity) ) ) ) ) ) 


(defmethod  (executor  move-in-lift) 

0 

(let*  ( (dt  (-  T3  time)) 

(deaired-poa  (Band  aelf  ilift-poa-deaired) ) 

(■  (third  deaired-poa) ) ) 

(send  self  tmove-del  deaired-poa  leg-poa-wrt-body  dt) 
(aetf  ready-poa 

(liat  (firat  ready-poa)  (second  ready-poa)  a)))) 


(defmethod  (exaoutor  ilift-poa-deaired) 

;  rnturena  pcaition-wst-body  which  will  be  at  the  end  of  lift  state. 

;  global  f  :  to-body-tranaform, 
l  global  v  :  inv-Hl 
0 

(let*  ( (leg-poa-wrt-earth  (to-earth-transform  HI  leg-poa-wrt-body)) 

(deaired-height  (+  lift-height  (third  (send  owner  :terrain-point  leg-poa-wrt-ea 

h>)>)) 

(to-body-traneform  inv-Hl  (list  (firat  leg-poa-wrt-earth) 

(aecond  leg-poa-wrt-earth) 
das: rad-height) ) ) ) 


(defmethod  (executor  :move-in-raady) 
0  ' 

(aetf  leg-poa-wrt-body  ready-poa)) 


(defmethod  (executor  move-in-return) 

0 

1  Modifying  log-pos-i  ia  redundant  but  it  can  correct  disturbance  by  itself, 
(let  ( (dt  (-  T4  time) ) 
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(dasirad-pos  raady-poa)) 

(send  self  move-del  desired-poa  leg-poa-wrt-oody  dt) ) ) 


(dafmethod  (executor  move-in-support) 

0 

/  globala  :  body-trana-ratel,  body-rotate-ratel 
;  lib  :  vaotaddf  magvaot 

I  In  general  terrain,  leg-poa-r  should  ba  updatad  by  seal  terrain  Haight, 
(lat  ( (leg-velooity-wrt-body  (aand  self  : fir>d-velocity-wrt-body) ) ) 

(aatf  lag-poa-wrt-body 

(vectudd  (magvaot  sampling-time  lag-valooity-wrt-body) 
lag-poa-wrt-body) ) ) ) 


(dafmathod  (executor  :initti> 

(leg-name  init-H) 

(aat f  aanaor  (aand  ownar  :oontaot-aansor) > 

(lat  <<x  (araf  init-K  0  3)) 

<y  (araf  init-H  13)) 

(t  (araf  init-H  23))) 

(eond  ((equal  lag-nama  'lagl) 

(satf  raady-poa  '(5  3  -4)) 

(aatf  lag-poa-wrt-body  (liat  6  3  (-  «))> 

(aatf  desired-foothold-poa-wrt-earth  (list 
((aqual  lag-nama  'lag2) 

(aatf  raady-poa  '(5-3  -4)) 

(aatf  lag-poa-wrt-body  (lift  5-3  (-■))> 
(aatf  daairad-foothold-poa-wrt-aarth  (liat 
((aqual  lag-nama  'lag3) 

(aatf  raady-poa  '(0  3-4)) 

(aatf  lag-poa-wrt-body  (liat  0  3  (-  a))) 

(aatf  daairad-foothold-poa-wrt-aarth  (liat 
((aqual  lag-nama  'lag4) 

(aatf  raady-poa  '(  0-3-4)) 

(aatf  lag-poa-wrt-body  (liat  0-3  (-a))) 
(aatf  daairad-foothold-poa-wrt-aarth  (list 
((aqual  lag-nama  'lagS) 

(aatf  raady-poa  '(-5  3  -4)) 

(satf  lag-poa-wrt-body  (liat  -5  3  (-  *))) 

(aatf  daairad-foothold-poa-wrt-aarth  (liat 
((aqual  lag-nama  'lag6) 

(satf  roady-pos  '(-5  -3  -4)) 

(aatf  lag-poa-wrt-body  (liat  -5  -3  (-  r>)> 
(aatf  daairad-foothold-poa-wrt-aarth  (list 


(+  x  «)  (+  y  3)  0)  )) 

<  +  x  5)  (-  y  3)  0)  )) 

(+  x  0)  (+  y  3)  0) )  > 

(  +  x  0)  (-  y  3)  0) ) ) 

(-  x  5)  (+  y  3)  0) ) ) 

(-  x  5)  (-  y  3)  0) ))  )) 


i 


1 


£-load-t-444 , liap  Shu  Nov  29  11:29:28  1990 

in  Mode: Common-Lisp;  Base:  10 

.***#*#********#*******************************<***************«**** 

l  load  file  <f-load-t-444.1isp) 

; 

t*************************************************!****** ************ 


l  graph-terrain  is  used  in  sensor  and  vision 
> 

i  Overlapped  working  volume  (1  foot) 
i  Front  and’  sear  are  not  extended. 

; 

;  logic  change 
/ 

(load  "robotikwak. robot /math-t" ) 

(load  "robotikwak. robot/uaer-interfaee-t2") 

(load  " robot  t  kwak . robot ; graph-t 1 " ) 

(load  " robot : kwak . robot / diaplay-t2 " ) 

(load  "robot : kwak. robotl; viaion-t") 

(load  "robotikwak . robots /ditoh-viaion-t" ) 

(load  "robot i kwak. robot ;tkm-t") 

(load  "robot i kwak. robot 4 /overlap-tkm-t"> 

(load  "robotikwak.robotl;foothold-t") 

(load  "robot : kwak. robot4;overlap-foothold-t”) 

(load  "robotikwak. robot ;aeneor-t") 

(.toad  "robot  i  kwak .  robot  1 ;  exeout or-t " ) 

(load  "robot i kwak . robot ; oontrol-maohine-t" ) 

(load  "robotikwak . robot /plim-machine-t") 

(load  "robot i kwak . robotl / leg-t " ) 

(load  "robot : kwak. robot 4 /over lep-leg-t") 

( load  " robot : kwak . robot S / test -over lap-leg-t-44 1 " ) 

(load  "robotikwak. robot; atability-t2") 

(load  "robot : kwak. robot; aupport-plane-t") 

(load  "robot : kwak . robot ; h-calculator-t") 

(load  "robot : kwak. robot ;command-regulat or-t") 

(load  "robot : kwak. robot; ter rain-regulator-t") 

(load  "robot : kwak . robot ; body-cont roller-t " ) 

(load  "robot : kwak. robot 1/body-t") 

(load  "robot : kwak, robot  6; atop-Dody-t") 

(load  "robotikwak. robotl/robot-tl") 

(load  "robot : kwak . robot 4 /overlap-robot-t") 

(load  "robot :kwal:. robot5;test-overlap-robot-t-442") 
(load  "robot : kwak. robot 6 /ditch-robot -t") 

(load  "robot : kwak. robot 6; robot 4 44") 

(load  " robot ; kwak . robot ; add-to-aystam-menu " ) 

(setf  asv  (maka-inatance  'ditch-robot!) 
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111  Mod* : Common-Lisp ;  Base: 10  -*- 

^*********«*******»***»#****»*j>«**»*»***********««**»*«i************ 

; 

,*  foothold-f indar  dafinition 
/ 

j****W****fr*#* ****************************************************** 


(def flavor  foothold-f indar (sixtaan-footholds 

four-linas  thm-ealeulator 
(no-oall-availabl«-f lag  nil) 
(TKM-margin  0.4)  ownar) 


0 

:  initable-instanca-variebles) 


(dafmathod  (foothold-f indar  :initti) 
(lag-name) 

(oond  ((equal  lag-nama  ' legl) 

(satf  aixtaen-footholda 

' ( (  7.3  4.3)  (  7.3 

(  6.3  4.3)  (  6.3 

!  3.3  4.3)  (  5.3 

(  4.3  4.3)  (  4.3 


(satf  four-linas 

'  ( ( (0  0.3420  -0.9397) 

(<0  -0.3420  -0.9397) 
(<0  -0.3420  -0.9397) 
((0  0.3420  -0.9397) 

( (aqual  lag-nama  *lag2) 

(satf  sixtaen-footholds 

'  ((  7.3  -4.3)  <  7.3  -3 
(  6.3  -4.3) 

(  3.3  -4.3) 

(  4.3  -4.3) 

(satf  four-linas 

'  (  (  (0  0.3420  -0.9397) 

((0  -0.3420  -0.9397) 
((0  -0.3420  -0.9397) 
((0  0.3420  -0.9397) 

((equal  lag-nama  'lag3) 

(satf  sixtaan-footholds 


.3)  (  7.3 
.3)  (  6.3 
.3)  (  3.3 
.3)  (  4.3 

(  6.0632 
(  6.0332 
(  3.4167 
<  3.4167 


2.3) 

2.3) 

2.3) 

2.3) 


(  7.3 
(  6.3 
(  5,3 
<  4.3 


1.3) 

1.3) 

1.3) 

1.3)  > ) 


.3)  (  7.3 
(  6.3  -3.3)  -  6.3 
3)  <  5.3 
.3)  (  4.3 


3.3  -3. 

4.3  -3. 


(  8.0632 
(  8.0832 
(  3.4167 
(  3.4167 


2.7339  0)> 
2.7339  0)) 
2.7339  0)> 
2.7339  0))))) 


-2.3)  (  7.3  -1.3) 
-2.3)  (  6.3  -1.3) 
-2.3)  (  3.3  -1.3) 
-2.3)  (  4.3  -1.3) ) ) 

-2.7339  0)) 

-2.7339  0)) 

-2.7339  0)) 

-2.733?  0) ) ) ) ) 


' ( (  1.5  4.3) 

(  1.5 

3.3) 

(  1.5 

2.3) 

(  1.5 

1.3) 

(0.5  4.3) 

(  0.5 

3.3) 

(  0.5 

2.3! 

(  0.5 

1.3) 

(-0.5  4.3) 

(-0.5 

3.3) 

(-0.5 

2.3) 

(-0,5 

1.3) 

(-1.5  4.3) 

four-linas 

(-1.5 

3.3) 

(-1.5 

2.3) 

(-1.5 

1.3)) ) 

'(((0  0.3420 

-0.9397)  ( 

2.2915 

2.7339  0)) 

(<0  -0.3420 

-0.9397)  ( 

2.2915 

2.7339  0) ) 

((0  -0.3420 

-0.9397)  <- 

2.2915 

2.7339  0)) 

((0  0.3420 

-0.9397)  (- 

2.2915 

2.7339  0) ) ) ) ) 

.  lag-nama  'lag4) 
sixtaan-footholds 

'  ((  1.5  -4.3) 

(  1.5 

-3.3) 

(  1.5 

-2.3) 

(  1.5 

-1.3) 

(  0.5  -4.3) 

(  0.5 

-3.3) 

(  0.5 

-2.3) 

(  0.5 

-1.3) 

(-0.5  -4.3) 

(-0,5 

-3.3) 

(-0.5 

-2.3) 

(-0.5 

-1.3) 

(-1.5  -4.3) 

(-1.5 

-3.3) 

(-1.5 

-2.3) 

(-1.5 

-1.3))  ) 

(aati  four-linas 


'  (((0  0.3420  -0.9397) 

((0  -0.3420  -0.9397) 
((0  -0.3420  -0,9397) 
((0  0.3420  -0.9397) 


(  2,2915  -2,7339  0)) 

(  2.2915  -2.7339  0)) 
(-2.2915  -2.7339  0)) 
(-2.2915  -2.7339  0) ) ) ) ) 
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((equal  leg-name  'leg5) 
(setf  sixteen-footholds 


'<<-4.0  4.3) 

(-4.0  3.3) 

(-4.0 

2.3) 

(-4.0 

1.3) 

(-5.0  4.3) 

(-5.0  3,3) 

(-5.0 

2.3) 

(-5.0 

1.3) 

(-6.0  4.3) 

(-6.0  3.3) 

(-6.0 

2.3) 

(-6.0 

1.3) 

(-7.0  4.3) 

(-7.0  3,3) 

(-7.0 

2.3) 

(-7.0 

1.3) ) ) 

(setf 

four-lines 
'(<(0  0.3420 

((0  -0.3420 
((0  -0.3420 
({0  0.3420 

-0.9397)  (- 
-0.9397)  <- 
-0.9397)  <- 
-0.9397)  (- 

3.3332 

3.3332 

7.8332 

7.8332 

2.7339  0) ) 

2.7339  0>) 

2.7339  0)) 

2.7339  0))))) 

( (equal 
(setf 

.  leg-name  'leg 6) 
sixteen-footholds 
'(<-4.0  -4.3)  (-4.0  -3.3) 

(-4.0 

-2.3) 

(-4.0 

-1.3) 

(-5.0  -4.3) 

(-5.0  -3.3) 

(-5.0 

-2.3) 

(-5.0 

-1.3) 

(-6.0  -4.3) 

(-6.0  -3,3) 

(-6.0 

-2.3) 

(-6.0 

-1.3) 

(-7.0  -4.3) 

(-7.0  -3,3) 

(-7.0 

-2.3) 

(-7.0 

-1.3))) 

(setf  four-lines 


'(((0 

0.3420 

((0 

-0.3420 

((0 

-0.3420 

((0 

0.3420 

-0.9397)  (-3.3332 
-0.9397)  (-3.3332 
-0.9397)  (-7.8332 
-0.9397)  (-7.8332 


-2.7339  0)) 
-2.7339  0) ) 
-2.7339  0)) 
-2.7339  0))))) 


) 

(setf  tkm-oaloulator  (send  owner  itkm-caleulator) ) 
) 


(defmethod  (foothold-finder  » find-foothold) 

<H6  inv-Hfi  body-trans-ratelO  body-rotat*-rat*10 
eatimated-support-plane) 

i  return*  ( (max-foothold  max-t)cm)  (foothold-list)  (tkm-list) ) 

I  all  points  are  wpt  body  coordinate  system. 

(let*  ( (estimated-support-plane-wrt-body 

(plane-transform  eatimated-support-plane  K6 ) ) 

(four-points  (send  self 

: four-points-on-support-plane 
four-lines  estimated-support-plane-wrt-body) ) 
(possible-footholds  (send  self 

sget-possible-footholds 
(send  self 

: estimate-footholds 

four-points  eat imated-s upport -plane -wrt -body ) 
H6  inv-H6) ) ) 

(send  self 

iget-foothold-with-max-TKH 
posaible-iootholda  H6 
body-trans-ratelO  body-rotate-ratelO) ) ) 


i  foothold-finder . find-foothold 

•  H *********  ********************************************* M 
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(defmethod  (foothold-finder  restimate-footholds) 

(four-points -wrt -body  estimated-support-plane-wrt-body) 

;  returns  eatimate-f ootholds-wrt-body 

(do*  ((footholds  sixtean-footholds  (edr  footholds)) 

(out-footholds  nil) 

(a-foothold  nil) ) 

( (null  footholds) 

(get-points-on-support-plana  out-footholds  estimated-aupport-plane-wrt-body) ) 
(setf  a-foothold  (cat  footholds)) 

(if  (in-side-o£-polygon  a-foothold 

(pick-two-diroenaions  four-pointa-wrt-body) ) 

(setf  out-footholds  (cona  a-foothold  out-footholds))))) 


(defmethod  (foothold-finder  sfour-pointa-on-aupport-plane) 

(four-lines  estimated- support -plane- wrt -body) 

;  returns  four  points  which  are  intersected  by  four-lines  on 
;  estimated-support -plane-wrt-body 
t  math  lib:  plane-intersection 

(do*  ((lines  four-lines  (cdr  linos)) 

(points  nil)) 

((null  lines)  points) 

(setf  points  (cons  (plane-intersection  (car  lines) 

estimatod-support -plane- wrt -body) 


points) ) ) ) 


(defmethoa  (foothold-finder  sget-foothold-with-max-TKM) 
(possible-footholds  H 

body-trans-rate  body-rotate-rate) 

;  returns  ( (max-foothold  max-tkm)  (focthold-list)  (t)an-list) ) 

;  sets  no-cell-available-flag 
i  real-footholds  is  really  possible  footholds 

(do  ((footholds  possible-footholds  (cdr  footholds)) 

(max-foothold  nil)  (a-foothold  nil)  (TKM-list  nil)  (a-TKM  nil) 
(real-footholds  nil)  <max-7KM  -100.0)) 

( (null  footholds) 

(setf  no-cell-available-flag  (<  max-TKM  TKM-margin) ) 

(if  ( >-  nutx-TKM  TKM-margin) 

(make -out put -f orm 

max-foothold  max-TKM  real-footholds  TKM-list  H) 
nil)  ) 

(setf  a-foothold  (car  footholds) ) 

(setf  a-TKM  (send  tkr/.-calculator  :find-tkm 

a-foothold  body-trana-rate  body-rotate-rate)) 

(if  a-TKM 

(progn  (serf  TKM-llst  (cons  a-TKM  TKM-list)) 

(setf  real-f ootholds  (cons  a-foothold  real-f ootholds) ) 
(if  (>  a-TKM  max-T!M) 

(progn  (setf  max -TKM  a-TKM) 

(aetf  max-foothold  a-foothold))))))) 


(defmothod  (foothold-f irider  :get-po3sioie-£octholds> 
(estimated-focthoids  H  inv-H) 

;  returns  possible-footholds  wrt  boay 
( co-body-t ransf orm  inv-H 

(send  self  : f ind-possibie-fcotholds 

( to-ea rth-t ransf o rm  H  est imated- f oot ho  ids ) ) 
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*  m  *  ★★  in  *  *********#******WiiMi*iir*******#*****************#**N 

;  foothold-finder .  estimate -foothold 

j  ******************************************************** M 


(defun  check-polarity  (pointl  point2  point3) 

(lot*  ( (voetl  (vootaub  point?  pointl)) 

(voct2  (vootaub  point3  pointl))) 

(if  (not  (third  voetl) ) 

(progn  (aotf  voetl  (revarae  (con?  0  (reverao  voetl)))) 

(aotf  voot2  (ravorae  (cona  0  (reverao  vect2)))))) 
(croaaprod  vootl  voot2) ) ) 


(dofun  get-pointa-on-support -plane  (point?  estimated-aupport-plane-wrt-body) 

;  roturna  intoraootion  pointa  with  support  plane  in  z-body  dircotion. 

;  math  lib:  plane-interaection 

(do*  ((points  pointa  (odr  points)) 

(out-points  nil) ) 

((null  points)  out-points) 

(aotf  out-points  (eons  (piano-intersection 

(make-line-to-get-point-on-support -piano 
(car  points) ) 

oatimated-support-plano-wrt-body)  out-points) ) ) ) 


(defun  in-sido-of-polygon  <a-point  polygon-points) 

;  polygon-points  must  be  convert -polygon  and  in  order  «  two  dimensional  points, 
(do*  ((first-points  polygon-points  (edr  first-points)) 

(second-points  (reverse  (cons  (car  first-points) 

(reverse  (edr  first-points)))) 

(edr  second-points)) 

(signs  nil)  (first-point  nil)  (second-point  nil)) 

((null  first-points)  (same-polarity  signs)) 

(aotf  first-point  (car  first-points)) 

(setf  second-point  (cat  second-points) ) 

(setf  signs  (cons  (chocx-polarity  first-point  second-point  a-point) 
signs) ) ) ) 


(defun  make-line-to-get -point-on-support-plane  (a-point) 

;  a-point  is  two  dimensional  point. 

;  returns  a-lino  ( (s-direction)  (a-point  -100)) 

(list  '(00  1)  (list  (first  a-pointi  (second  a-point)  -100))) 


(defun  pick-two-dimensions  (points) 

(if  (listp  (first  points)) 

(do*  ((point.*  points  (edr  points))  ;  more  than  one  point  case 

(a-point  nil) 

(out-points  nil ) ) 
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(  (null  points)  out-points) 

(setf  a-point  (car  points)) 

(setf  out-points  (cons  (list  (first  a-point)  (second  a-point)) 

out-points) ) ) 

(list  (first  points)  (second  points))))  ;  one  point  case 


(defun  same-polarity  (signs) 

(do  ((signs  (cdr  signs)  (cdr  signs)) 

(first-sign  (plusp  (third  (car  signs)))) 

(same  T) ) 

((null  signs)  same) 

(if  (not  (equal  first-sign  (plusp  (third  (car  signs))))) 
(setf  same  nil) ) ) ) 


*************  AW*****************************************'1 

;  foothold-finder. find-foothold. get-foothold- with-MAX-thm 

;»  ***«  ***»  «*«**»1****1IM****************  ******  *********  ******  II 


(defun  malce-output-form 

(max-foothold  max-TKM  possible-footholds  TKM-list  K) 
/  output-form  :  ( (foothold-with-max-tkm  tkm) 
i  (leg-pro jeeted-permitted-footholds) 

;  < leg-pro jeoted-TKM-list) ) 

;  output  footholds  ere  in  earth  coordinate. 
i  math  lib  :  to-earth-transform 

(list  (list  (to-earth-transform  H  max-foothold)  max-TKM) 
(to-earth-transform  H  possible-footholds) 

TKM-list) ) 


*’'*ft********#****+*******#******************+#*4**w*A***H 

i 

;  foothold-finder. select -foothold. get -possible- foothold 


»• 


(defmethod  (foothold-finder  :find-possible-footholds) 
(estimated-footholds-wrt -earth) 

;  returns  possible-footholds-wrt -earth 
;  graph-terrain  is  object. 

(do*  ((footholds  estimated-footholds-wrt-earth  (cdr  footholds)) 
(a-foothold  nil)  (t-cell  nil)  (out-footholds  nil)) 

( (ni-11  footholds)  (unique-footholds-only  out-footholds)) 
(setf  a-foothold  (car  footholds)) 

(setf  t-cell  (get-center-of-digitired-terrain-cell  a-foothold) ) 
(setf  out-footholds 

;  (cont.  (list  (+  (first  t-oell)  0.5) 
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;  (+  (second  t -call)  0.5) 

;  0.0)  out-footholds)))) 

(if  (send  owner  iparmitted-cell  t-cell) 

(setf  out -footholds 

(cons  (Sr/nd  owner  iterrain-point  t-eell) 
out-fcotholds))))) 


(defun  qet-centar-of-digitixad-terrtin-cell  (e-foothold) 
;  cell  resolution  is  1  foot  by  1  foot 

(list  (+  (floor  (first  e-foothold))  0.5) 

(+  (floor  ('second  e-foothold))  0.5))) 


(defun  unique-footholds-only  (mixed-footholds) 

(do*  ((footholds  mixed-footholds  (cdr  footholds)) 
(out-footholds  nil) 

(e-foothold  nil) ) 

((null  footholds) , out-footholds) 

(setf  e-foothold  (oar  footholds)) 

(if  (not  (member  e-foothold  out-footholds  itest  'equal)) 
(setf  out-footholds  (cons  e-foOthold  out-footholds))))) 


graph-tl . lisp 


Wed  Nov  28  10:10:16  1980 


1 


tit  -*-  Mode jCommon-Lisp;  PackagesUSER;  Base:10  -*- 

•  ********************************************************** 

; 

;  low  level  graph  routines 
/ 


(defvar  “robot -display-window*  nil) 
(defvar  “robot-dieplay-window-array*  nil) 
(defvar  “robot-window*  nil) 

(defvar  “robot-window-array*  nil) 

(defvar  “robot-window-width*  nil) 

(defvar  “robot -window-height*  nil) 

(defvar  “terrain-buffer*  nil) 

(defvar  “terrain-buffer-array*  nil) 
(defvar  *max-y*  nil) 

(defvar  “start-point*  nil) 

;ff  1 

(defvar  “xs“  (make-array  2)) 

(defvar  “ys“  (make-array  2)) 


(defun  oopy-terrain-to-robot-window  () 

(tv : aheet-f orce-aooess  ( “robot -window* ) 

(send  “robot -window*  sbitblt 
tvialu-ior  “robot-window-width*  “robot-window-height* 
“terrain-buffer-array*  2200))) 


(defun  draw-to  (a-point  a-window) 

;  global  variables  t  “start-point* 

(tv : aheet-f orce-aoceas  (a-window) 

(send  a-window  *  :draw-line  (first  *start -point*) 
(-  *max-y*  (second  “start-point*)) 

(first  a-point) 

(-  *max-y*  (second  a-point) )  tv:alu-ior)) 
(setq  *atart-point*  a-point)) 


(defun  draw-to-d  (a-point  a-window) 

;  global  variables  :  “start-point* 

(tv: sheet-force-access  (a-window) 

(setf  (aref  *xs*  0)  (+  4  (first  *start-point*) ) ) 

(setf  (aref  *xs*  1)  (+  4  (first  a-point))) 

(setf  (aref  *ya*  0)  (+  4  (-  *max-y*  (second  “start-point*)))) 

(setf  (aref  *ya*  1)  (+  4  (-  “max-y*  (second  a-point)))) 

(send  a-window  :draw-wide-curve  “xs*  *ya*  2)) 

(setq  “start-point*  a-point)) 


(defun  erase-to  (a-point  a-window) 
t  global  variables  :  ‘start-point* 

(tv: sheet-forco-access  (a-window) 

(send  a-window  '  :draw-.\ine  (first  “start-point*) 
(-  *max-y*  (second  ‘start-point*)) 

(first  a-point) 

(-  * max-y *  (second  a-point))  tv:alu-andca; ) 
(setq  “start-point*  a-point) ) 
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(defun  get-keyboard-input  () 

i  This  is  not  for  tha  graphics,  but  this  function  uses  Zeta  LISP. 
i  This  is  the  reason  why  this  function  is  in  Zeta  graphic  package, 
(sand  "terminal-io*  (tyi-no-hang) ) 


(defun  make-robot-window  () 

(setq  •robot-display-window*  (tv make-window 

'tv swindow 
:blinker-p  nil 
(position  ' (0  0) 
iwidth  *screen-width* 

{height  (truncate  (•  0.8  * screen-height*) ) 
{borders  2 

{label  "robot-display-window" 

{name  "robot-display-window" 

: save-bits  t 
texpcsa-p  t)) 

(let*  ( (r-w  (send  *robot-display-window*  {width)) 

(r-h  (send  *robot-diaplay-window*  (height)) 

(r-x  nil)  (r-y  nil) ) 

(multiple-value  (r-x  r-y)  (send  "robot-display-window*  {position)) 

(setq  *robot-window*  (tv:make-window  ' 

tv (window 

{position  (list  r-x  r-y) 

{width  r-w 
{height  r-h 
sblinker-p  nil 
{borders  2 

{label  "robot-window" 

{name  "robot-window" 
t save-bits  t 
{expose-p  nil)) 

(setq  "terrain-buffer*  (tv make-window 

' tv {window 

{position  (list  r-x  r-y) 

(width  r-w 
(height  r-h 
:blinker-p  nil 
{borders  2 

{label  "terrain-buffer" 

{name  "terrain-buffer” 

:save-bits  t 
:expose-p  nil) ) 

(setq  *max-y*  (send  "robot-window*  : inside-height) ) ) 

(setq  *robot-diaplay-window-array*  (send  *robot-display-window*  (bit-array) ) 
(setq  *robot-window-array*  (send  *robot-w,indow*  :bit-array) ) 

(setq  *robot-window-width*  (send  "robot-window*  : inside-width) ) 

(setq  *robot-window-height*  (send  "robot-window*  : inside-height) ) 

(setq  "terrain-buffer-array*  (send  "terrain-buffer*  (bit-array) ) ) 


(defun  make-visible  () 

(send  "robot-display-window*  ibitblt 
tv:alu-seta  *robot-window-width*  ‘robot-window-height* 
*robot-window-array*  2200)) 
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(dafun  mova-to  (a-point) 

;  global  variables  :  •start-point* 

;  This  function  just  changes  *start-point* . 
(setq  *atart-point*  a-point)) 


(dafun  sava-tarrain-to-tarrain-bu££ar  < ) 

(tv: ahaat-foroa-accasa  <*terrain-bu££er*) 

(sand  *terrain-buffer*  ibitblt 
tv:alu-sata  *robot-window-width*  *robot-window-haight* 
*robot-window-array*  2  2  0  0))) 


(dafun  claar-robot-window  () 

(tv: shaat-foroa-acoass  (*robot-window*) 
(sand  *robot -window*  iclaar-window) ) ) 


(dafun  middle-of-robot-window  () 

(list  (/  (sand  *robot-window*  iinsida-width)  2) 

(/  (sand  *robot-window*  sinaida-baight)  2))) 


(dafun  kill-robot-tarrain-windows ( ) 
(sand  *robot-display-window*  skill) 
(sand  * robot -window*  skill) 

(sand  *tarrain-buffar*  skill)) 
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;/;  -*-  Mode: Common-Lisp;  Base: 10  -*- 
. ******************************************************************* 

! 

t  H-oalculetor  definition 

l 

I  an* **************************************************************** 


(dei'flavor  H-calculator ( (sampling-tims  0.1)  H 

old-H) 

0 

: initable-instance-variables) 


(defmethod  (H-calculator  sinitti) 

0 

;  library  fuention  :  idant 
(setf  H  (idant)) 

(satf  (araf  H  0  3)  6.5) 

(satf  (araf  H  1  3)  19. 5) 

(satf  (araf  H  2  3)  9.4) 

H) 


(dafmathod  (H-oaloulator  :naw-H) 

(body-trans-rate  bedy-rotate-rate) 

(aatf  H 

(orthogonalization 

(get-new-H 

H 

(gat-dal-H 

H 

(gat-dalta  body-trana-rata  body-rotate-rate  sampling-time)))))) 


(dafmathod  (H-calculator  taava) 
0 

(satf  old-H  H) ) 


(dafmethod  (H-calculator  : restore) 
0 

(satf  H  old-H) ) 


;  H-oalculator .new-H 

i  * ***•*••**••**»•*****•*** ******************************* 


(defun  get-delta  (body-trans-rate  body-rotate-rate  sampling-time) 
(let*  ( (del-trana-x  (*  (first  body-trans-rate)  sampling-time)) 

(del-trana-y  ('  (second  body-trans-ratc)  sampling- time) ) 

(del-trans-r  (*  (third  body-trans-rate)  sampling-time)) 

(del-rotate-x  ('  (first  body-rotate-rate)  sampling-time)) 

(del-rotate-y  (*  (aecond  body-rotate-rate)  sampling-time)) 
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(del-rotata-z  <*  (third  body-rotato-rate)  sampling-time) ) ) 
(list  (list  del-trans-x  dal-trans-y  del-trana-z) 

(list  del-rotate-x  del-rotate-y  del-rotate~z) ) ) ) 


(dafun  get-del-H  (H  delta-trans-rotate) 
t  math  lib  :  ident 

(let*  ( (H-del  (ident))  /  initialze  identity  matirix 

(delta-trans  (first  delta-tsans-rotate) ) 
(delta-rotate  (saoond  delta-trans-rotate) ) ) 


setf 

(aref 

H-del 

0 

0) 

0) 

setf 

(sref 

H-del 

1 

0) 

(third  delta-rotate)) 

setf 

(aref 

H-del 

2 

0) 

<-  (saoond  delta-rotate) ) ) 

setf 

(aref 

H-del 

0 

1) 

(-  (third  delta-rotate))) 

setf 

(aref 

H-del 

1 

1) 

0) 

setf 

(aref 

H-del 

2 

1) 

(first  delta-rotate)) 

setf 

(aref 

H-del 

0 

2) 

(second  delta-rotate)) 

setf 

(aref 

H-del 

1 

2) 

(-  (first  delta-rotate))) 

setf 

(aref 

H-del 

2 

2) 

0) 

setf 

(aref 

H-del 

0 

3) 

(first  delta-trans)) 

setf 

(aref 

H-del 

1 

3) 

(second  delta-trans)) 

setf 

(aref 

H-del 

2 

.3) 

(third  delta-trans)) 

setf 

(aref 

H-del 

3 

3) 

0) 

(matrixmult 

H  H-del) ) ) 

(dafun  get-new-H  (H  del-H) 
(matrixadd  H  del-H) ) 
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Mode : Common-Lisp;  Base: 10  -*- 


/  leg  flavor  definition 
/ 


(defflavor  leg  {name  owner  plan-machine  control-machine 

executor  oontaat-aensor  tkm-calculator 
foothold-finder  exshanged-leg 
foothold  tkm  foothold-liat  tkm-liat  tkra-p 
reserved-foothold  reserved-tkm) 

0 

: initable-inat anca-variablea 
tgettable-instanee-variables) 


(defmethod  (leg  tinitti) 

(H) 

(aetf  oontact-sensor  (make-instance  'contact-sensor  lownar  self)) 

(setf  exeoutor  (make-instance  'executor  (owner  self)) 

( set  £  control-machine  (make -instance  ' oontrol-atate-machine  (owner  self)) 
(setf  plan-maohine  (make-instance  'plan-state-machine  towner  self)) 
(aetf  tkm-aalculator  (make-instance  ' tkm-caloulator  (owner  self)) 

(setf  foothold-finder  (make-instance  'foothold-finder  (owner  self)) 

(setf  foothold  (send  executor  tinitti  name  H) ) 

(send  contact-sensor  linitti  name) 

(send  control-machine  tinitti  name) 

(send  plan-maohine  tinitti  name) 

(send  tkm-caloulator  tinitti  name) 

(send  foothold-finder  tinitti  name)) 


(defmethod  (leg  t contact-confirm) 

0 

(send  contact-sensor  :contact-p)) 


(defmethod  (leg  t do-planned-mot ion) 

0 

(send  plan-machine  : change) 

(send  control-machine  (change) 

(send  exeoutor  (move  (send  owner  :get-Hl)  (send  owner  tget-inv-Hl) 
(send  owner  ( get-body-trens-ratel) 

(send  owner  iget-body-rotate-ratel) ) 

(send  contact-sensor  (sensing)) 


(defmethod  (leg  :get-Hl) 
0 

(send  owner  tget-Kl)) 


(defmethod  (leg  :has-fcothold-p) 
0 

foothold) 
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(dafmathod  (lag  : interlock-confirm) 

0 

l  may  add  stable-without-p  self 

(if  (sand  exchangad-leg  :contact-conf irm) 
t 

nil) ) 


(dafmathod  (lag  : leg-pos-wrt-body) 

0 

(sand  axaoutor  : leg-pos-wrt-body) ) 


(dafmathod  (lag  : lift-able) 

0 

(if  (equal  (sand  plan-maohine  : state-name)  'eligible-to-lift) 

self 

nil)) 


(dafmathod  (leg  :lift-ok) 

0 

(aand  owner  :lift-ok  name)) 


(dafmathod  (lag  : lifted) 

0 

(sand  owner  > lifted  name)) 


(dafmathod  (lag  :naw-foothold) 
0 

(cond  ((car  foothold-list) 
(sand  self  : set-max) 
t) 

(t 

nil))) 


(dafmathod  (lag  :parmittad-cell) 
<t-call) 

(sand  owner  ipermitted-cell  t-oall) ) 


(dafmathod  (leg  :plac«-able) 

0 

t  check  plan  state  as  wail,  as  foothold  for  the  lag 

(if  (equal  (aund  plan-machine  : state-name)  'available-lag) 
self 
nil) ) 


(dafmathod  (leg  :projected-pos) 
() 
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(send  executor  :get-desired-pos> ) 


(defmethod  (leg  : select-foothold) 

0 

l  out-list:  ( (max-foothold  max-tkm)  (foothold-list)  (tkm-list)) 
(let*  ((H  (send  owner  :get-H$)) 

(inv-H  (send  owner  :get-inv-HS) ) 

(body-trana-rate  (send  owner  :get-body-trana-ratelO) ) 
(body-rotate-ratt  (send  owner  :get-body-rotete-retelO) ) 
(estimated-support-plane 

(send  owner  tget-estimated-support-plane) ) 

(out-list 

(send  foothold-finder  : find-foothold 

H  inv-H  body-trans-rate  body-rotate-rate 
eatimatad-aupport-plane) ) ) 

(setf  foothold  (first  (first  cut-list))) 

(setf  reserved-foothold  foothold) 

(setf  tkm  (second  (first  out-list))) 

(setf  reserved-tkm  tkm) 

(setf  foothold-list  (seoond  out-list) ) 

(setf  tkm-list  (third  out-'liaft) ) ) ) 


(defmethod  (leg  s sand-decision) 

(a-deoision) 

(send  plan-machine  t send-deoision  a-deoision) 
;  ) 


i (defmethod  (leg  i send-deoision  t after) 

/  (a-deoision) 

(if  (equal  a-deoision  'place) 

(send  exeoutor  : aet-desired-pos  foothold))) 


(defmethod  (leg  : send-exchange) 
(a-leg) 

(setf  exchenged-leg  a-leg) ) 


(defmethod  (leg  : set-max) 

0 

(do  ((footholds  (cdr  foothold-list)  (cdr  footholds!) 
(tkms  (cdr  tkm-list)  (cdr  tkms)) 

(max-foothold  (car  foothold-list)) 

(max-tkm  (oar  tkm-list) ) 

(out-footholds)  (out-tkms) ) 

((null  footholds) 

(setf  foothold  max-foothold) 

(setf  tkm  max-tkm)  ( 

(setf  foothold-list  out-footholds) 

(setf ‘ tkm-list  out-tkms)) 

(oond  ( (>  (car  tkms)  max-tkm) 

(setf  max-foothold  (car  footholds)) 

(setf  max-tkm  (oar  tkms))) 

(t 

(setf  out-footholds 

(cons  (car  footholds)  out-footholds) ) 
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(eetf  out-tkms 

(cons  (car  tkms)  out-tkms)))))) 


(defmathod  (leg  :stable-witliout-p) 

0 

(send  owner  i stable-without-p  self)) 


(defmethod  (leg  (supporting) 

0 

,  (oond  ( (equal  (send  plan-machine  istate-name)  'planned-contact) 
self) 

((equal  (send  plan-machine  ;state-name)  'eligible-to-lift) 
self) 

(t  nil)) 

) 


(defmethod  (lag  :supporting-p) 

0 

(cond  ((equal  (send  control-machine  i state-name)  'contact) 
self) 

((equal  (send  control-machine  : state-name)  'support) 
self) 

(t  nil)) 

) 


(defmethod  (lag  (terrain-point) 
(t-cell) 

(send  owner  (terrain-point  t-cell) ) 


(defmethod  (leg  :TKM-limit) 
0 

(cond  ( (null  tkm) 

»ei ;; 

( (<  tkm  0.1) 
self) 

(t 

nil))) 


(defmethod  (leg  ;TKM-limit-p) 
0 

(cond  ((null  tkm-p) 

self)  i 

( (<  tkm-p  0 .5) 
self) 

(t  nil))) 


(defmethod  (leg  (update -tkm) 
0 
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(let  (  (body-trans-rste  (send  owner  iget-body-trana-ratelO) ) 

(bcdy-rotate-rate  (send  owner  :get-body-rotate-ratelO) ) 
(inv-H  (send  owner  :get-inv-H10) > ) 

(set*  tkm  (send  tkm-ealculator  sfind-tkm 

(to-body-transform  inv-H  foothold) 
body-trans-rate  body-rotate-rate) ) ) 

) 


(defmethod  (leg  :updat«-tltm-p) 

0 

(let  ( (body-trans-rate-p  (send  owner  iget-body-trans-ratel) ) 

(body-rotate-rato-p  (send  owner  :got-body-rotate-ratel) ) 
(inv-H-p  (send  owner  :get-inv-Hl) ) ) 

(setf  tlcm-p  (send  t)cm-calculator  :find-t)cm 

(to-body-transform  inv-H-p  foothold) 
body-trans-rate-p  body-rotate-rate-p) ) ) 

) 


(defmethod  (leg  iwith-foothold) 

0 

(oond  (reserved-foothold 

(setf  foothold  reserved-foothold) 

(setf  tkm  reserved-tkm) * 

self) 

(t  nil))) 


I 
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;;;  -*-  Mod# : Common-Lisp;  Package :USER;  Base; 10  -*- 

; 

;  robot  math  library 
i 

j *******************  I************************************************ 


(defun  arc-cos  (s) 
(acoa  a>  ) 


(defun  col-mul(mat  coll  col2) 

(let  ( (sum  0) ) 

(dotimes  (i  4) 

(setf  sum  (  +  sum  (*  (aref  mat  i  coll)  (aref  mat  i  col2))))) 
sum) ) 


(defun  oounting(a-.list) 

(do  ( (a-list  a-list  (cdr  a-liat)) 
(i  0  (+  i  1))) 

( (null  a-list)  i) ) ) 


(defun  crossprod  (vectl  vect2) 

(let*  (<xl  (first  veotl))  (x2  (first  vect2) ) 

(yl  (second  vectl) )  (y2  (second  vect2) ! 

(zl  (third  vectl))  (s2  (third  vect2) ) 

(x  (-  <*  yl  z2 )  <*  y2  zl) ) ) 

<y  (-  (*  x2  zl)  (*  xl  z2)  )  ) 

(z  (-  (*  xl  y2)  (*  x2  yl)))) 

(list  x  y  z) ) ) 


(defun  delete-list  (a-list  b-list)  ;  delete  a-list  from  b-list 
(do  ((deleting-list  a-list  (cdr  deleting-list)) 

(deleted-list  b-list)) 

((null  deleting-list)  deleted-list) 

(setf  deleted-list  (remove  (car  deleting-list) 

deleted-list  :test  'equal)))) 


(detmacro  dequeue  (queue) 

Mprogl  (car  ,  queue) 

(setf  , queue  (cdr  , queue)))) 


(defun  dotprod  (vectl  vect2) 

;  No  dimension  limitation  ! ! ! 

(apply  '+  (mapcar  '*  vectl  vect2))) 


(defmacro  enqueue  (queue-name  element) 
;  globals  :  queue-namo 
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,*  Value  of  recover  field  of  command  is  a  list. 

;  Two  recover  command  is  possible  for  one  sampling-time. 

;  structure  of  QUEUE  :  (first  second  third  . . .  last) 

' (setq  , queue-name  (nconc  , queue-name  (list  , element)))) 


(defmacro  emptyqueue  (queue) 
'  (setq  , queue  •  0 ) ) 


(defun  ident() 

(make-array  '  (4  4) : initial-contents 
'  ( (1  0  0  0) 

(0  10  0) 

(0  0  10) 

<0  00  1)))) 


(defun  magnitude  (a-vector) 

(sqrt  (dotprod  a-vector  a-vector) ) ) 


(defun  magvect  (const  v«ct) 

;  magvect  -  const  *  vect 

(mapoar  *' (lambda  (u-element) 
(*  const  a-elament)) 
vect) ) 


(defun  matrixadd  (mtl  mt2) 

(let  ( (mt3  (idcnt) ) ) 

(dotimes  (i  4) 

(dotimes  (j  4) 

(setf  (aref  mt3  i  j)  (+  (aref  mtl  i  j)  (aref  mt2  i  j))))) 
mt3> ) 


(defun  matrixinv (mat) 

(let  ( (px  (-  (col-mul  mat  03))) 
(py  (-  (col-mul  mat  13))) 
(pz  (-  (col-mul  mat  23))) 
(matrix  (transpose  mat))) 
(setf  (aref  matrix  3  0)  0)  (setf 
(setf  (aref  matrix  3  2)  0)  (setf 
(setf  (aref  matrix  0  3)  px)  (setf 
(uetf  (aref  matrix  2  3)  pz) 
matrix) ) 


(aref  matrix  3  1)  0) 
(arof  matrix  3  3)  1) 
(aref  matrix  1  3)  py) 


(defun  matrixmult  (mtl  mt2) 

(let  ((mat  (make-array  '(4  4))))  ;it  defines  0  through  3.  (4  is  not  included) 
(dotimes  (i  4)  ;  will  repeat  i-0,  1,  2,  and  3.  (not  4) 

(dotimes  (j  4) 

(setf  (aref  mat  i  j)  0)  ;  initialize  to  zero 

(dotimes  (k  41 

(setf  (aref  mat  i  j)  (  +  (aref  mat  i  j)  (*  (aref  mtl  i  k) 


l 
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mat) ) 


(aref  mL2  k  j >))))) ) 


(dafun  nil-list (a-liat) 

(do  ( (a-list  a-list  (cdr  a-liat)) 
(not-nil  nil) ) 

((nail  a-liat)  (not  not-nil)) 
(if  (car  a-liat) 

(setf  not-nil  t) ) ) ) 


(defun  normalixe-vector  (a-vaotor) 

(lat  < (m  (magnitude  a-vaotor))) 

(if  (<  m  0.0000001) 

(list  000) 

(magvect  (/  l.o  m)  a-vactor)))) 


(dafun  orthogonalixation  <mt) 

l  Oram-Sohimit  orthogonalisation  process 
(let*  ( (mx  (ident)) 

(tx  (araf  mt  0  3))  (ty  (araf  mt  1  3))  (ti  (araf  mt  2  3)) 


(xl  (araf  mt  0  0))  (x2  (araf  mt  0  1))  (x3  (aref  mt  0  2)) 

(yl  (araf  mt  1  0))  (y2  (araf  mt  1  D)  (y3  (araf  mt  1  2)) 

(zl  (araf  mt  2  0))  (z2  (araf  mt  2  1))  (z3  (araf  mt  2  2)) 

(ml  (magnitude  (list  xl  yl  zl)>) 

(xl  (/  xl  ml) ) 

(yl  (/  yl  ml) ) 

(xl  (/  xl  ml) ) 

(a  (dotprod  (list  xl  yl  xl)  (list  x2  y2  *2))) 

(x2  (-  x2  (*  a  xl))) 

(y2  <-  y2  (*  a  yl))) 

(z2  (-  z2  (*  a  xl))) 

(m2  (magnitude  (list  x2  y2  z2))) 

<x2  (/  x2  m2)  ) 

(y2  (/  y2  m2) ) 

(x2  (/  z2  m2))) 


(setf 

(aref 

mx 

0 

0) 

xl) 

(setf 

(aref 

mx 

0 

1) 

x2) 

(setf 

(aref 

mx 

0 

2) 

x3> 

(setf 

(araf 

mx 

1 

0) 

yi) 

(setf 

(aref 

mx 

1 

1) 

y2> 

(setf 

(aref 

mx 

1 

2) 

y3> 

(setf 

(araf 

mx 

2 

0) 

xl) 

(setf 

(aref 

mx 

2 

1) 

X?) 

(setf 

(araf 

mx 

2 

2) 

■  3) 

(setf 

(aref 

mx 

0 

3) 

tx) 

(setf 

(aref 

mx 

1 

3) 

ty) 

(setf 

(araf 

mx 

2 

3) 

tz) 

mx) ) 


(dafun  plane-transform  (  plana  matrix  ) 

;  Tl insf ormed-Plane  -  Plana  *  Matrix 

;  plana  is  defined  as  ( (a  b  c)  d) .  (a  b  c)  is  unit  normal,  d  is  -(distance), 
(let*  ( (naw-a  nil) 

(new-b  nil) 

(new-c  nil)  , 

(naw-d  nil) 

(old-unit-normal  (oar  plane)) 

(old-d  (cadr  plane)) 

(old-a  (first  old-unit-normal) ) 

(old-b  (second  old-unit-normal  i  > 

(•'id-c  (third  old-unit-normal)) 

(mag  nil) ) 

(swtf  new-a  (+  (*  old-a  (aref  matrix  00))  (*  old-b  (aref  matrix  1  0)) 
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(*  old-c  (aref  matrix  2 
<aat£  naw-b  (+  (*  old-a  (aref  matrix  0 
(*  old-c  <ar e£  matrix  2 
(setf  new-c  (+  (*  old-a  (aref  matrix  0 
(*  old-c  (araf  matrix  2 
(setf  new-d  (+  (*  old-a  (araf  matrix  0 
(*  old-c  (araf  matrix  2 
(satf  mag  (magnitude  diet  new-a  new-b 
(if  (<  (abs  mag)  0.0000001) 

(print  "Error  in  PlsneTransform" ) 
(list  (liat  (/  naw-a  mag)  (/  naw-b 
(/  naw-d  mag) ) ) ) ) 


0)))) 

1) )  (*  old-b  (aref  matrix  1 

1)))) 

2) )  (*  old-b  (aref  matrix  1 

2)))) 

3) )  (*  old-b  (aref  matrix  1 
3))  old-d) ) 

naw-c) ) ) 


mag)  (/  new-c  mag)) 


1>> 

2)) 

3)) 


(defun  plane-distance  (plana  velocity  poaition) 

;  Plana  (X  -  Q)N  -  0  ,  atraight  line  X  ■  P  +  tA. 

/t"(Q-P)N/(AN)  if  A  is  normalised  than  t  ia  signed  dietanoe. 
;  if  t  ia  infinitive  than  plane-distance  raturnas  nil. 

;  plane-distance  returns  t. 

(let*  ((A  (normalise-vector  velocity)) 

(N  (first  plane)) 

(dis  (-  (second  plane))) 

(Q  (magvect  dis  N) )  ;  magvaot  ■  const  *  vector 
(P  position) 

(Q_p  (vectaub  Q  P) ) 

(AN  (dotprod  A  N) ) 

(numerator  (dotprod  QJ?  N) ) ) 

(if  (<  (aba  AN)  0.0000001)^  ;  no  crossing 
nil  ;  returns  nil 

(/  numerator  AN) ) ) ) 


(defun  plane-inf eraection  (a-line  a-plane) 

;  a-line  ( (direction)  (point))  X  -  P  +  tA. 

;  a-plane  ((unit-normal)  -dist)  (X  -  Q)N  -  0. 

(let*  ((velocity  (normalize-veotor  (first  a-line) )) 

(position  (second  a-line) ) 

(t-valua  (plane-distance  a-plsne  velocity  position))) 
(if  t-value 

(veotadd  position  (magvect  t-value  velocity)) 
nil)))  ;  no  intersection 


(defun  plane-normal-distance  (a-plane  a-point) 

;  vector-type-plane  (abed) 

;  paul- type -point  tranepoae(x  y  z  1) 

(let*  ((unit-normal  (first  a-plane)) 

(dis  (second  a-plane)) 

(veotor-type-plane  (reverse  (cons  dis  (reverse  unit-normal) )) ) 
(paul-type-point  (reverse  (cons  1  (reverse  a-point))))) 
(dotprod  vector-type-plane  paul-type-point) ) ) 


(defun  rotatamat (axis  angle)  ;  array  index  starts  from  0  not  1. 
;  return  rotatematrix  angle  : radian  axis  :  x  y  or  z 
(let  ( (mat  (ident) ) 
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(cosa  (cos  angle) ) 

(sina  (sin  angle) ) ) 

(case  axis 

(x-axis 

(setf  (acef  mat  1  1) 

(setf  (acef  mat  2  1) 

(y-axis 

(setf  (aref  mat  0  0) 

(setf  (aref  mat  2  0) 

(z-axie 

(setf  (aref  mat  0  0) 

(setf  (aref  mat  1  0) 

mat))  ;  returns  this  value. 


cosa) 

(aetf 

(aref 

mat 

1 

2) 

(-  sina) ) 

sina) 

(setf 

(aref 

mat 

2 

2) 

cosa) ) 

cosa) 

(setf 

(aref 

mat 

0 

2) 

sina) 

-  sina)) 

(setf 

(aref 

mat 

2 

2) 

cosa) ) 

cosa) 

(setf 

(aref 

mat 

0 

1) 

(-  sina)) 

sina)  (setf  (aref  mat  1  1)  cosa))) 


(defun  to-body-tranaforro  (inv-H  points-wrt-earth) 

/  returns  points-wrt-body 

(if  (listp  (first  points-wrt-earth))  ;  test  multi-points 

(do  ((points  points-wrt-earth  (cdr  points))  ;  multi-points  case 
(out -points  nil) ) 

((null  points)  (reverse  out-points)) 

(setf  out-points  (oons  (transform  inv-H  (oar  points))  out-points))) 
(transform  inv-H  points-wrt-earth)))  ;  single  point  case 


(defun  to-earth-transform  (H  points-wrt-body) 

;  returns  points-wrt-earth 

(if  (listp  (first  points-wrt-body))  ;  test  multi-points 

(do  ((points  points-wrt-body  (odr  points))  >  multi-points  case 
(out-points  nil)) 

((null  points)  (reverse  out-points)) 

(setf  out-points  (oons  (transform  H  (oar  points) )  out-points) ) ) 
(transform  H  points-wrt-body)))  ;  single  point  oase 


(defun  transform(mat  point)  ;  array  index  starts  from  0  not  1. 
(let  ( (x  (car  point)) 


(y  (cadr  point) ) 

(z  (if  (caddr  point)  (caddr  point)  0))) 


(Hat  (  + 

(*  x 
(aref 

(aref  mat  0 
mat  0  3) ) 

0) ) 

<* 

y 

(aref 

mat 

0 

1)  ) 

(* 

z 

(aref 

mat 

0 

2)  ) 

(  + 

(*  x 

(aref 

(aref  mat  1 
mat  1  3)) 

on 

<* 

y 

(aref 

mat 

1 

1)  ) 

<* 

2 

(aref 

mat 

1 

2)  ) 

(  + 

<*  x 
(aref 

(aref  mat  2  0) ) 
mat  2  3))))) 

/* 

y 

(aref 

mat 

2 

1)  ) 

(* 

1 

(aref 

mat 

2 

2)  ) 

(defun  transmat  (x  y  s) 

;  returns  translational  marix 
(let  ((matrix  (idant))) 

(setf  (aref  matrix  0  3.)  x) 
(setf  (aref  matrix  1  3)  y) 
(setf  (aref  matrix  2  3)  z) 
matrix) ) 


(defun  transpose  (mat) 

(let  ((matrix  (make-array  '(4  'll))) 
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(dotimes  (i  4) 

(dotimes  (j  4) 

(setf  (aref  matrix  i  j)  (aref  mat  j  i)))) 
matrix) ) 


(defun  unit-croasprod  (vectl  vect2) 

>  generate  unitnormal  vector  of  vectl  X  veet2 
(let*  ( (xl  (first  vectl))  (x2  (first  vect2)) 

( y 1  (second  vectl))  (y2  (second  vact.2)) 

(il  (third  vectl))  (z2  (third  vect2)) 

(x  <-  (*  yl  *2)  (*  y2  *1))) 

(y  (-  (*  x2  zl)  (*  xl  z2) ) ) 

(z  (-  <*  xl  y2)  (*  x2  yl))) 

(m  (aqrt  (+  (*  x  x)  <*  y  y)  {*  z  z)>>)) 

(list  (/  x  m)  (/  y  m)  (/  z  m) ) ) ) 


(defun  vectadd  (vectl  ve,ct2) 
>  vectsub  -  vectl  +  vect2 
i  no  limit  in  dimension 
(mapcas  '+  vectl  veot2)) 


(defun  vectsub  (vectl  vect2) 
/  vectsub  -  veotl  -  vect2 
/  no  limit  in  dimension 
(mapcar  vectl  vect2)) 
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it)  -*-  Mode: Common-Lisp;  Base:10  -*- 

j******************************************************************* 

; 

l  ovarlap-foothold-finder  definition 

« 

t 

;*****************************»*******«******»*****»************'  ■  *  ■ 

(defflavor  overlap-foothold-finder (adjacent-leg-numbers) 
(foothold-finder) 
iinitable-inatanoe-variablas) 


(defmethod  (overlap-foothold-finder  iinitti) 
(leg-name) 

(cond  ((equal  leg-name  'legl) 

(setf  adjaoent-leg-numbers  '(3)) 
(setf  sixteen-footholds 


' <(  9.0  4.3) 

(  9.0  3.3) 

(  9.0  2.3) 

(  9.0 

1.3) 

(  8.0  4.3) 

(  8.0  3.3) 

(8.0  2.3) 

(  8.0 

1.3) 

<  7.0  4.3) 

(  7.0  3.3) 

(7.0  2.3) 

(  7.0 

1.3) 

<  6.0  4.3) 

(  6.0  3.3) 

(  6.0  2.3) 

(  6.0 

1.3) 

(  5.0  4.3) 

(  5.0  3.3) 

(  5.0  2.3) 

(  5.0 

1.3) 

(  4.0  4.3) 

(  4.0  3.3) 

(4.0  2.3) 

(  4.0 

1.3) 

(3.0  4.3) 

(  3.0  3.3) 

(3.0  2.3) 

(  3.0 

1.3)  )) 

(setf 

four-lines 

'(<(0  0.3420 

-0.9397)  ( 

9.5  2.7339 

0)) 

<<0  -0.3420 

-0.9397)  ( 

9.5  2.7339 

o>> 

<<0  -0.3420 

-0.9397)  < 

2.5  2.7339 

0)) 

((0  0.3420 

-0.9397)  < 

2,5  2.7339 

0>>))> 

((equal  leg-name  'leg2) 

(setf 

adjacent-leg-numbers  '(4)) 

(setf 

sixteen-footholds 

'  <<  9.0  -4.3) 

(  9.0  -3.3) 

<  9.0  -2.3) 

(  9.0 

-1.3) 

(  8.0  -4.3) 

(  8.0  -3.3) 

(  8.0  -2.3) 

(  8.0 

-1.3) 

(  7.0  -4.3) 

(  7.0  -3.3) 

(  7.0  -2.3) 

(  7.0 

-1.3) 

(  6.0  -4.3) 

(  6.0  -3.3) 

(  6.0  -2.3) 

(  6.0 

-1.3) 

(  5.0  -4.3) 

(  5.0  -3.3) 

(  5.0  -2.3) 

(  5.0 

-1.3) 

(  4.0  -4.3) 

(  4.0  -3.3) 

(  4.0  -2.3) 

(  4.0 

-1.3) 

(  3.0  -4.3) 

(  3,0  -3.3) 

(  3.0  -2.3) 

(  3.0 

-1.3)  ) ) 

(setf 

four-lines 

'(((0  0.3420 

-0.9397)  ( 

9.5  -2.7339 

0)  ) 

((0  -0.3420 

-0.9397)  ( 

9.5  -2.7339 

0)  ) 

((0  -0.3420 

-0.9397)  { 

2.5  -2.7339 

0)  j 

((0  0.3420 

-0.9397)  ( 

2.5  -2.7339 

0))))) 

((equal  log-name  'leg3) 

(setf 

adjacent-leg-numbers  '  (1 

5)) 

(setf 

sixteen-footholds 

'  <  <  3.0  4.3) 

(3.0  3.3) 

(3.0  2.3) 

(  3.0 

1.3) 

(  2.0  4.3) 

(2.0  3.3) 

(  2.0  2.3) 

(  2.0 

1.3) 

(  1.0  4.3) 

(  1.0  3.3) 

(1,0  2.3) 

(  1.0 

1.3) 

(  0.0  4.3) 

(0.0  3.3) 

(  0.0  2.3) 

(  0.0 

1.3) 

(-1.0  4.3) 

(-1.0  3.3) 

(-1.0  2.3) 

(-1.0 

1.3) 

(-2.0  4.3) 

(-2.0  3.3) 

(-2.0  2.3) 

(-2.0 

1.3) 

(-3.0  4.3) 

(-3.0  3.3) 

(-3.0  2.3) 

(-3.0 

1.3)  )  ) 

(Sv-tf 

four-lines  , 

'(((0  0.3420 

-0.9397)  ( 

3.5  2.7339 

0)  ) 

- 

( (0  -0.3420 

-0.9397)  < 

3.5  2.7339 

0)  ) 

((0  -0.3420 

-0.9397)  (- 

3.5  2.7339 

0)) 

((0  0.3420 

-0.9397)  (- 

3.5  2.7339 

0))))) 

((equal  leg-name  'leg4) 

(setf  adjacent-leg-numbers  '(2  6)) 
(setf  sixteen-footholds 


'<(  3.0  -4.3)  (  3.0  -3.3)  (  3.0  -2.3)  (  3.0  -1.3) 
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(  2.0 

-4.3) 

(  2.0 

-3.3) 

(  2.0 

-2.3) 

(  2.0 

-1.3) 

(  1.0 

-4.3) 

(  1.0 

-3.3) 

(  1.0 

-2.3) 

(  1.0 

-1.3) 

(  0.0 

-4.3) 

(  0.0 

-3.3) 

(  0.0 

-2.3) 

(  0.0 

-1.3) 

(-1.0 

-4.3) 

(-1.0 

-3.3) 

(-1.0 

-2.3) 

(-1.0 

-1.3) 

(-2.0 

-4.3) 

(-2.0 

-3.3! 

(-2.0 

-2.3) 

(-2.0 

-1.3) 

(-3.0 

-4.3) 

(-3.0 

-3.3) 

(-3.0 

-2.3) 

(-3.0 

-1.3) )  ) 

(setf  four-lines 

'<(<0  0.3420  -0.9397)  <  3.5  -2.7339  0) ) 

{<0  -0.3420  -0.9397)  <  3.5  -2.7339  0)) 

{(0  -0.3420  -0.9397)  (-3.5  -2.7339  0)) 

<<0  0.3420-0.9397)  (-3.5-2.73390))))) 

((equal  leg-name  'leg5) 

(setf  adjacent-leg-numbers  '  (3) ) 

(setf  sixteen-footholds 


<<-3.0 

4.3) 

(-3.0 

3.3) 

(-3.0 

2.3) 

(-3.0 

1.3) 

(-4.0 

4.3) 

(-4.0 

3.3) 

(-4.0 

2.3) 

(-4.0 

1.3) 

(-5.0 

4.3) 

(-5.0 

3.3) 

(-5.0 

2.3) 

(-5.0 

1.3) 

(-6.0 

4.3) 

(-6.0 

3.3) 

(-6.0 

2.3) 

(-6.0 

1.3) 

(-7.0 

4.3) 

(-7.0 

3.3) 

(-7.0 

2.3) 

(-7.0 

1.3) 

(-8.0 

4.3) 

(-8.0 

3.3) 

(-8.0 

2.3) 

(-8.0 

1.3) 

(-9.0 

4.3) 

(-9.0 

3.3) 

(-9.0 

2.3) 

(-9.0 

1.3))) 

(setf  four-lines 

'(((0  0.3420-0.9397)  (-2.5  2.73390)) 

((0-0.3420-0.9397)  (-2.5  2.73390)) 

((0  -0.3420  -0.9397)  (-9.5  2.7339  0>) 

((0  0.3420-0.9397)  (-9.3  2.73390))))) 

((equal  leg-name  'leg6) 

(setf  adjacent-leg-numbers  '(4)) 

(setf  sixteen-footholds 

'<<“3.0  -4.3)  (-3.0  -3.3)  (-3.0  -2.3)  (-3.0  -1.3) 

(-4.0  -4.3)  (-4.0  -3.3)  (-4.0  -2.3)  (-4.0  -1.3) 

(-5.0  -4.3)  (-5.0  -3.3)  (-3.0  -2.3)  (-5.0  -1.3) 

(-6.0  -4.3)  (-6.0  -3.3)  (-6.0  -2.3)  (-6.0  -1.3) 

(-7.0  -4.3)  (-7.0  -3.3)  (-7.0  -2.3)  (-7.0  -1.3) 

(-8.0  -4.3)  (-8.0  -3.3)  (-8.0  -2.3)  (-8.0  -1.3) 

(-9.0  -4.3)  (-9.0  -3.3)  (-9.0  -2.3)  (-9.0  -1.3))) 

(setf  four-lines 

'(((0  0.3420  -0.9397)  (-2.5  -2.7339  0)) 

((0  -0.3420  -0.9397)  (-2.5  -2.7339  0)) 

((0  -0.3420  -0.9397)  (-9.5  -2.7339  0)) 

((0  0.3420-0.9397)  (-9.5-2.73390))))) 

) 

(setf  tkm-calculator  (send  owner  :tkm-calculator) ) 

) 


(defmethod  (overlap-foothold-finder  :get-possible-footholds) 
(estimated-footholds  H  inv-H) 

;  returns  possible-footholds  wrt  body 
i  find-possible-footholds  function  tests  obstacles 
(to-body-transform 
inv-H  i 

(send  self  ;get-rid-of-overlap 

(sand  self  : find-possible-footholds 

(to-earth-tranaform  H  estimated-footholds))))) 


(defmethod  (overlap-foothold-f inder  :get-rid-of-overlap) 
(foot holds -wrt -earth) 
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(let*  ( (a jacent-legs 
(mapcar 

#' (lambda  (leg-num) 

(.send  owner  : nth-leg  leg-num)  ) 
adjacent-leg-numbers) ) 

(ad jacent-legs -in-possible-interaction 
(remove 
nil 

(mapcar  #'  (lambda  (leg) 

(if  (send  leg  :placa-abla)  /no  interaction 
nil 
log) ) 

a jacent-legs) ) ) ) 

(send  self  : remove-over lapped- foot hold 
footholds-wrt -earth 

adjacent-legs-in-possible-interaction) ) ) 


(defmethod  (overlap-foothold-finder  : remove-over lapped-foothold 
(footholds-wrt -earth  legs-in-poaaible-interaction) 
(do  ((legs  legs-in-posaible-interaetion  (cdr  legs)) 
(out-footholds  footholds-wrt-earth) 

(oveilap-foothold) ) 

((null  legs)  out-footholds) 

(setf  overlap-foothold  (send  (car  legs)  ifoothold)) 

(setf  out-footholds 

(remove  overlap-foothold 
out-footholds 
/test  #' (lambda  (xl  x2) 

(send  self  :overlap-p  xl  x2)) 

)))) 


(defmethod  (overlap-foothold-finder  loverlap-p) 
(xl  x2) 

(let  ((xl-integer  (mapcar  #' truncate  xl) ) 
(x2-integer  (mapcar  #' truncate  x2) ) ) 
(equal  xl-integer  x2-intager) ) ) 


l 
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i:;  -*-  Mode : Common-Lisp;  Base: 10  -*- 
/ 

/  overlap-leg  definition 

f 


(def flavor  overlap-leg  () 
(leg) 

) 


(defmethod  (overlap-log  iinitti) 

(H> 

(setf  contact-sensor  (make-instance  ' contact-senaor  :owner  self)) 

(setf  exeoutor  (make-instanoe  'executor  : owner  self)) 

(setf  control-machine  (make-instanee  ' control-state-machine  towner  self)) 
(setf  plan-machine  (make-instance  'plan-state-machine  : owner  self)) 
(setf  tkm-calculator  (make-instance  'overlap-tkm-calculator  :owner  self)) 
(setf  foothold-finder  (make-instance  'overlap-foothold-finder  towner  self)) 
(setf  foothold  (send  exeoutor  linitti  name  H)) 

(send  contact-sensor  iinitti  name) 

(send  control-maohine  sinitti  name) 

(send  plan-machine  iinitti  name) 

(send  tkm-calaulator  iinitti  name) 

(send  foothold-finder  iinitti  name)) 


(defmethod  (overlap-leg  mth-leg) 
(leg-num) 

(send  owner  tnth-leg  leg-num)) 


(defmethod  (overlap-leg  : foothold) 
0 

foothold) 


I 
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;/;  Mode tCommon-Lisp;  Base:  10 
;  overlap-robot  definition 

J  *  *  **********************  if********************#*********!************ 


(def flavor  overlap-robot (> 
(robot) 

: initable-instance-variableJ 
: gettable-instanoe-varis.blBS) 


(defmethod  (overlap-robot  iinitti) 
0 


(send  graph-asv  tinit-data) 

(setf  vision-system  (make-instance  'vision-system  i owner 
(send  vision-system  tinitt-') 

(setf  joystick  (make-instance  'joystick)) 

(send  joystick  ireaet) 

(empty-queue  lift-queue) 

(setf  lift-flag  t) 

(let  ((H)) 

(setf  body  (make-instance  'body  iowner  self)) 

(setf  H  (send  body  iinitti)) 

(setf  legs  (list 

(make-instance  'overlap-leg  : name  ' legl 
(make-instance  'overlap-leg  tname  '  leg2 
(make-instance  'overlap- leg  :name  ' leg3 
(make-instance  'overlap-leg  tname  'leg4 
(make-instance  'overlap-leg  tname  'legs 
(make-instance  'overlap-leg  tname  ' leg6 


self) ) 


.-owner  self) 
: owner  self) 
towner  self) 
t owner  self) 
towner  self) 
towner  self) 


)) 

(mapoar  #' (lambda  (a-leg)  (send  a-leg  iinitti  H) )  legs)) 


) 


(defmethod  (overlap-robot  :nth-leg) 
(leg-num) 

i  nth  starts  counting  from  zero. 

;  leg-num  starts  from  on®. 

(nth  (-  leg-num  1)  legs) ) 
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;;  -*-  Mode :Common-Lisp;  Base:  10 

**********  l 

overlap-tkm-calculator  definition 

******************************************************************* 


(daf flavor  overlap-tkm-calculator  <> 
(tkm-calculator) 

: initable-inatance-variables ) 


(defmethod  (overlap-ttan-oaloulator  sinitti) 
(leg-name) 

(cond  ((equal  leg-name  '  legl) 


(aatf  wo Eking- volume 


' ((<(0  0  1)  3.316) 

<  <1 

0 

0) 

-9.5) 

<<o 

0.9397 

0  0420) 

(((0  0  1)  5.7313) 

<  (1 

0 

0) 

-2.5) 

((0 

0.9397 

-0.  j420) 

((equal  leg-name  'leg2) 
(aetf  working-volume 

'  <  <<<0  0  1)  3.316) 

<<1 

0 

0) 

-9.5) 

((0 

0.9397 

0.3420) 

(((0  0  1)  5.7313) 

<  (1 

0 

0) 

-2.5) 

((0 

0.9397 

-0.3420) 

((equal  leg-name  'leg3) 
(aetf  working-volume 

'  ((<(0  0  1)  3.316) 

<<1 

0 

0) 

-3.5) 

((0 

0.9397 

0.3420) 

(((0  0  1)  5.7313) 

<<1 

0 

0) 

3.5) 

((0 

0.9397 

-0.3420) 

((equal  leg-name  'leg4) 
(aetf  working-volume 

'(((<0  0  1)  3.316) 

<<1 

0 

0) 

-3.5) 

((0 

0.9397 

0.3420) 

(((0  0  1)  5.7313) 

<<1 

0 

0) 

3.5) 

((0 

0.9397 

-0.3420) 

((equal  leg-name  'legS) 
(aetf  working-volume 

' ( ( ( (0  0  1)  3.316) 

<<1 

0 

0) 

2.5) 

((0 

0.9397 

0.3420) 

((<0  0  1)  5.7313) 

<<i 

0 

0) 

9.5) 

((0 

0.9397 

-0.3420) 

((equal  lag-name  'leg6) 
(aetf  working-volume 

' ( <<<0  o  1)  3.316) 

<  <1 

0 

0) 

2.5) 

(  (0 

0.9397 

0.3420) 

(((0  0  1)  5.7313) 

(  (1 

0 

0) 

9.5) 

((0 

0.9397 

-0.3420) 

) 

) 


-2.569) ) 
-2.569))))) 


2.569)  ) 

2.569) )))) 


-2.569) ) 
-2.569))))) 


2.569)  ) 

2.569) )))) 


-2.569) ) 
-2.569))))) 


2.569) ) 
2.569) )  ) ) ) 
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lit  Mode :Common-Lisp;  Base: 10  -*- 


t **^*****^***^**^**^*^**********^******^*******^**  *  *  *  *  *  ** 

; 

/  plan-state  flavor  definition 

I 


(defflavor  plan-state  ((decision  nil)  (observation  nil)  (command  nil) 

(condition  nil)) 


(stnte) 

iinit able -instance- variables) 


(defmethod  (plan-state  : generate -command) 
0 

Command) 


(defmethod  (plan-state  : change) 

(given-decision  observed-state  given-condition) 
(cond  ( (and  decision  (liatp  decision) ) 

(cond  ((equal  given-decision  (first  decision)) 
(first  next-state)) 

( (equal  given-decision  (second  decision) ) 
(second  next-state) ) 

(t  self))) 

(condition 

(if  (and  (equal  given-condition  condition) 

(equal  observed-state  observation)) 
next-state 
self) ) 

(t 

(cond  ( (equal  observed-state  observation) 
next-state) 

( (equal  given-decision  decision) 
next-state) 

(t  self) ) ) ) ) 


j  ***************  ***************************************** 

;  plan-state-machinie  flavor  definition 


(defflavor  plan-state-machine  ( (decision  nil)  (observation  nil) 

(condition  nil)  (lift-ready-flag  nil) 
,  control-machine) 


(state-machine) 

:  initable-instance-variablos) 


(defmethod  (plan-state-machine  :initti) 
(leg-name) 

(if  (member  leg-r.ame  '  (legl  leg4  leg5)  ) 
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(send  self  : init-plan-machine  ' eligible-to-lif t ) 
(send  self  : init-plan-machine  'available-leg)) 
(setf  control-machine  (send  owner  {control-machine) ) ) 


(defmethod  (plan-state-machine  : init-plan-machine) 

<a-state-name) 

(let  (available-leg  planned-contact  eligible-to-lift 
planned-lift  actual-lift  planned-exchange) 

(setf  actual-lift 

(make-instance  'plan-state 

:nan  'actual-lift 
iobc jrvation  'ready 
: command  'recover-command)) 

(setf  planned-lift 

(make-instance  'plan-state 

:name  'planned-lift  : condition  ' stable-without 
{observation  'support 
(next-state  actual-lift)) 

(aetf  planned-exchange 

(make-instance  'plan-state 

:name  'planned-exchange  {condition  'interlock-confirm 
■  {observation  'support 
{next-state  aetual-lift) ) 

(aetf  eligible-to-lift 

(make-instance  'plan-state 

{name  'eligible-to-lift 
{decision  ' (lift  exchange) 

inext-atate  (list  planned-lift  planned-exchange))) 

(setf  planned-contact 

(make-instance  'plan-state 

{name  'planned-contact  (observation  'oontact 
(command  'deploy -command 
inext-atate  eligible-to-lift)) 

(setf  available-leg 

(make-instance  'plan-state 

(name  'available-leg  {decision  'place 
:next-atate  planned-contact) ) 

(send  actual-lift  : set-next-state  available-leg) 


(aetf  state  (cond 


) 

) 


( (equal  a-state-name 
available-leg) 

( (equal  a-statc-name 
planned-contact) 

( (equal  a-state-name 
eligible-to-lift) 

( (equal  a-state-name 
planned-lift) 

( (equal  a-state-name 
planned-exchange ) 
((equal  a-state-name 
actual-lift) ) ) 


(send  available-leg  : state-name) ) 
(send  planned-contact  ; state-name) ) 
(send  eligible-to-lift  : state-name) ) 
(send  planned-lift  : state-name) ) 
(send  planned-exchange  : state-name) ) 
(send  actual-lift  :state-name) ) 


(defmethod  (plan-state-machine  .-change  ibefore) 

0 

(setf  observation  (send  control-machine  :state-name) ) 

(cond  ((and  (equal  (send  state  :state-name)  'planned-exchange! 
(send  owner  : interlock-confirm) 

(send  owner  :stable-without-p) 
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1  (send  owner  slift-ok)) 

;  (set f  lift-ready-flng  t) 

s  (aetf  condition  'interlock-confirm)) 

/  ((and  (equal  (send  state  latate-name)  'planned-lift) 

;  (aend  owner  : stable-without-p) 

I  (send  owner  :lift-o)c)) 

;  (aetf  lift-ready-flag  t) 

;  (aetf  condition  ' stable-without) ) 

t  <t 

;  (aetf  condition  nil) 

/  (aetf  lift-ready-flag  nil))) 

;  > 


(defmethod  (plan-etate-maohine  ichange) 

0 

(aetf  obaervation  (send  control-machine  t state-name) ) 

(cond  ((and  (equal  (send  state  tatate-name)  'planned-exchange) 
(send  owner  i interlock-confirm) 

(send  owner  tatable-without-p) 

(send  owner  tlift-ok)) 

(aetf  lift-ready-flag  t) 

(aetf  condition  'interlock-confirm)) 

((and  (equal  (send  state  istate-name)  'planned-lift) 

(send  owner  latable-without-p) 

(send  owner  ilift-ok)) 

(aetf  lift-ready-flag  t) 

(aetf  condition  'atable-without) ) 

<t 

(aetf  condition  nil) 

(aetf  lift-ready-flag  nil) ) ) 

(aetf  state  (send  state  ichange  decision  observation  condition) ) 

;  > 

; (defmethod  (plan-atate-maohine  i change  i after) 

>  0 

(aend  control-machine  : aend-command 

(aend  state  igenerate-coramand) ) 

(if  (and  lift-ready-flag 

(equal  (aend  aelf  : state-name)  'actual-lift)) 

(send  owner  : lifted))) 


(defmethod  (plan-state-machine  isend-decision) 
(a-decision) 

(aetf  decision  A-decision) ) 


i 
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Mode i Common-Lisp;  Base; 10  -*- 


t 

i  robot  flavor  definition 

f 

j ********************  ************************************ 


(defflavor  robot (legs  body  vision-system  joystick 
(J ift-able-lags  nil) 

(plaee-able-iega  nil)  (supporting-legs  nil) 
(supporting-p-legs  nil) 

(joy-command  MO  0  0))  lift-queue  lift-flag) 

0 

: initable-instanoe-variables 
igettable-inatanoe-variables) 


(defmethod  (robot  iinitti) 

0 

(sand  graph-aav  iinit-data) 

(e«tf  vision-system  (make-instance  'vision-system  towner  self)) 
(send  vision-system  iinitti) 

(setf  joystiok  (make-instance  'joystick)) 

(send  joystiok  : reset) 

(empty-quaue  lift-queue) 

(setf  lift-flag  t) 

(let  ((H)) 

(setf  body  (make-instance  'body  lowner  self)) 

(setf  H  (send  body  iinitti)) 

(setf  lege  (list 


(make-instance 

'  leg 

iname 

'legl 

i  owner 

self) 

(make-instance 

'leg 

:  name 

'  leg2 

: owner 

self) 

(make-inatance 

'leg 

i  name 

'  leg3 

lowner 

self) 

(make-instance 

'  leg 

iname 

'  leg4 

towner 

self) 

(make-instance 

'  leg 

iname 

'legs 

i  owner 

solf ) 

(make-instance 

'  leg 

iname 

'leg6 

: owner 

self) 

)  > 

(mapcar  #' (lambda  (a-leg)  (send  a-leg  iinitti  H) )  legs)) 

) 


(defmethod  (robot  ifind-lift-able-legs) 

0 

(delete  nil  (mapcar  #' (lambda  (a-leg)  (send  a-leg  :lift-able))  lags))) 


(defmethod  (robot  :find-place-able-legs) 

0 

(delete  nil  (mapcar  #' (lambda  (a-leg)  (send  a-leg  :place-able) )  legs))) 


(defmethod  (robot  i find-supporting-legs) 

0 

(delete  nil  (mapcar  #' (lambda  (a-leg!  (send  a-leg  isupporting) )  legs))) 
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(dafmathod  (robot  :£ind-aupporting-p-laga> 

0 

(dalata  nil  (mapear  #' (lambda  (a-leg)  (aand  a-lag  :aupporting-p) )  lega))) 


(dafmathod  (robot  :gat-body~rotata-ratal) 
0 

(sand  body  :gat-body-retata-ratal) ) 


(dafmathod  (robot  sgat-body-rotata-ratalO) 
0 

(aand  body  sgat-body-xotata-ratalO) ) 


(dafmathod  (robot  sgat-body-trana-ratal) 

0 

(aand  body  :gat-body-trana-ratal) ) 


(dafmathod  (robot  igat-body-trana-ratalO) 
0 

(aand  body  : gat -body-trana- ratal 0) ) 


(dafmathod  (robot  sgat-aatimatad-aupport-plana) 
0 

(aand  body  igat-aatimatad-aupport-plana) ) 


(dafmathod  (robot  igat-Hl) 

0 

(aand  body  :gat-Hl)) 


(dafmathod  (robot  :gat-H6) 
0 

(aand  body  :gat-H6) ) 


(dafmathod  (robot  igat-HIO) 

0 

(aand  body  :gat-H10)) 


(dafmathod  (robot  sgat-inv-Hl) 
0 

(aand  body  :gat-inv-Hl) ) 


(dafmathod  (robot  :gat-inv-H6) 
0 

(aand  body  :gat-inv-H6) ) 
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(dafmathod  (robot  :get-inv-H10) 
0 

(sand  body  »gat-inv-H10) ) 


'•! 

(dafmathod  (robot  tlift-ok) 

(lag-nama) 

(oond  (lift-flag 

(cond  (.{aqual  lag-nama  (aand  (first  lift-quaua)  mama 
(satf  lift-flag  nil) 
t) 

(t 

nil) )) 

(t  nil))) 


(dafmathod  (robot  iliftad) 

(lag-nama) 

(if  (aqual  lag-name  (aand  (firat  lift-quaua)  mama)) 
(dtquaua  lift-quaua) 

(print  (list  "arror  in  lifting"  lag-nama)))) 


(dafmathod  (robot  :parmittad-oall) 

(t-oall) 

(aand  viaion-syatam  :parmittad-oall  t-oall) ) 


(dafmathod  (robot  t scanning) 

0 

(aand  viaion-syatam  i scanning)) 


(dafmathod  (robot  : atabla-without-p) 

(a-lag) 

(sand  body  tatabla-p 

(ramova  a-lag  supporting-p-lags) ) ) 


(dafmathod  (robot  itarrain-point) 

(t-oall) 

(aand  viaion-syatam  itarrain-point  t-oall)) 


( 

;  prolog  intarfaoa  robot  methods 

i 

^*****rt«**r)  ft************************************************ 
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(dafmathod  (robot  jat-tkm-limit) 

0 

(1st  (( limit -lev 

(car  (delete  nil 

(mapoar  #' (lambda  (a-leg)  (send  a-leg  sTKM-limit))  lift-able-legs) ) ) ) ) 
(satf  supporting-lags  (remove  limit-lag 

supporting-lags) ) 

(satf  lift-abla-lags  (ramova  limit-lag 

lift-abla-lags) ) 

limit-leg) ) 


(dafmathod  (robot  teheok-atability-p) 

0 

(sand  body  istable-p-m  suppocting-p-lags  (first  lift-queue))) 


(dafmathod  (robot  lohaok-tkm-limit-p) 

0 

(dalata  nil 

(mapoar  #' (lambda  (a-lag)  (sand  a-lag  tTKM-limit-p) )  supporting-p-legs) ) ) 


(dafmathod  (robot  ido-reoovary) 

0 

(oar 

(dalata  nil 

(mapoar  #' (lambda  (a-lag)  (sand  a-lag  iwith-foothold) )  plaoa-abla-lags) ) ) ) 


(dafmathod  (robot  lexeoute-planned-motion) 

0 

(mapoar  #' (lambda  (a-lag)  (sand  a-lag  ido-plannad-motion) )  lags)) 


(dafmathod  (robot  igraphioal-display) 

0 

(sand  graph-aav  :diaplay  (sand  body  igat-Hl) 

(mapoar  #'(lambda  (a-lag)  (sand  a-lag  slag-pos-wrt-body) )  lags)) 


(dafmathod  (robot  s has-mora-tkm) 
(lagl  lag2) 

(-*  (sand  lagl  itknw 
(sand  lag2  :tkm) ) ) 


(dafmathod  (robot  : lag-with-new-foothold) 

0 

/  raturn  a-lag  with  new-foothold, 

(do  ( (naw-foothold-flags  (mapoar  #' (lambda  (a-lag)  (sand  a-leg  :naw-foothold) )  place-a! 
a-lags) 

(mapoar  #' (lambda  (a-lag)  (sand  a-lag  s new-foothold) )  placa-al 


a-lags) ) 

(a-lag  nil) ) 
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( (or  (nil-list  new-foothold-f lags) 

•-leg) 

(if  a-leg  a-leg  nil) ) 

(setf  a-leg  (send  self  ;max-sm-leg  nil)))) 


(defmethod  (robot  :max-sm-leg) 

(a-leg) 

i  max-am-leg  without  supporting  a-leg 
(let  (legs-with-foothold) 

(cond  (plaoe-able-legs 

(setf  legs-with-foothold 

(remove  nil  (mapcar  #' (lambda  (leg) 

(if  (send  leg  ihas-foothold-p) 
leg 
nil) ) 

plaoe-able-legs) ) ) 

(oond  (legs-with-foothold 

(do  ((legs  (edr  legs-with-foothold)  (cdr  lege)) 

(largest-leg  (oar  legs-with-foothold)  largest-leg) 
(temp-support-legs  (remove  a-leg  supporting-legs))) 

< (null  legs) 

(if  (send  body  istable  (oona  largest-leg  temp-support-legs)) 
largest-leg 
nil)) 

(if  (send  body  imore-stable  temp-support-legs 
(oar  legs)  largest-leg) 

(setf  largest-leg  (oar  legs))))) 

(t  nil))) 

(t  nil)))) 


(defmethod  (robot  imodify-oommand) 

0 

(send  body  imodify-oommand)) 


(defmethod  (robot  iwait-f or-lif t) 

0 

(delete  nil 

(mapoar  #' (lambda  (a-leg)  (send  a-leg  i lift-not-done) )  aupporting-p-lega) ) ) 


(defmethod  (robot  i read- joystick) 

0 

(let  ((joy-value  (send  joystick  iget-joy-value) ) ) 

(setf  joy-eommand 

(reverse  (odr  (reverse  (send  joystick  iget- joy-value) ) ) ) ) 
(if  (fourth  joy-value) 
nil 
t))) 


i 

(defmethod  (robot  : restore-command) 
0 

(send  body  : restore-command) ) 


(defmethod  (robot  [before  : send-deoision) 
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{ 1«9 1  leg2  a-decision) 
(cond  ( (equal  a-decision  'exchange) 
(enqueue  lift-queue  leg1..)) 
((equal  a-decision  'lift) 
(enqueue  lift-queue  legl)))) 


(defmethod  (robot  ssend-decisi^n) 

(legl  leg2  a-deeiaion) 

(oond  ((equal  a-decision  'exohange) 

(send  legl  tsend-deoision  a-decision) 
(send  leg2  saend-deciaion  'place) 

(send  legl  :aand-exohange  leg2) ) 

(t 

(send  legl  t sand-decision  a-decision)!)) 


(defmethod  (robot  ismallest-tkm-leg) 

0 

;  seleot  smallest-TKM-leg 
l  tkm  is  nil  or  positive 

(do  ((legs  (odr  lift-able-legs)  (edr  legs)) 

(amallest-leg  (car  lift-able-lega) ) 

(amallest-tkm  nil)  (tkm  nil) ) 

((null  legs)  amalleat-leg) 

(aetf  smalleat-tkm  (if  (send  amallest-leg  ttkm) 

(send  amalleat-leg  ttkm)  -1000)) 

(aetf  tlcm  (if  (aend  (oar  lege)  ttkm) 

(send  (oar  legs)  ttkm)  -1000)) 

(if  (>  amallest-tkm  tkm)  (aetf  amallest-leg  (oar  legs))) 

(if  (and  (aqual  amallest-tkm  -1000)  (equal  tkm  -1000)) 

"Error  t  more  than  one  legs  are  out  of  kinematio  limit"))) 


(defmethod  (robot  i a low-down- robot) 

0 

(send  body  islow-down)) 


(defmethod  (robot  tapeed-up-robot) 
0 

(send  body  tspeed-up)) 


(defmethod  (robot  : stable) 

0 

(send  body  istable  supporting-legs)) 


(defmethod  (robot  : stable  m) 

0  “  , 

(send  body  tstable-m  supporting-legs)) 


(defmethod  (robot  tatable-without) 

(a-leg) 

(send  body  tstable  (remove  a-leg  supporting-legs))) 
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(defmethod  {robot  :update-robot-atatus) 

0 

(setf  lift-flag  t) 

(aatf  lift-able-lags  (sand  salf  s find-lift-able-legs) ) 

(aatf  place-able-legs  (send  salf  ! find-place-able-lags) ) 

(aatf  supporting-legs  (send  self  : find-supporting-legs) ) 

(aatf  aupporting-p-lega  (sand  self  ifind-supporting-p-lags) ) 

(mapoar  #' (lambda  (a-lag)  (sand  a-lag  supdate-tkm-p) )  supporting-p-legs) 
(if  (aand  salf  s eheek-tkm-limit-p) 

(sand  body  jstop-motion  (sand  salf  scheck-tkm-limit-p) ) 

(sand  body  srastora-motion) ) 

(if  (not  (sand  salf  leheek-stability-p) ) 

(sand  body  tmodify-command-p) 

(sand  body  t  raatora-eornmand-p) ) 

(sand  body  :oaloulata-motion  joy-eommand  legs) 

(mapoar  #'  (lambda  (a-lag)  (sand  a-lag  .-salact-f oothold) )  place-able-legs) 
(mapoar  #' (lambda  (a-lag)  (sand  a-lag  lupdata-tkm) )  supporting-lags)) 


(dafun  oraata-terrain  () 

(sand  graph-terrain  icraata)) 


(dafun  kill-terrain () 

(sand  graph-terrain  skill)) 


;*«f***#******#!lf******«r*********llM»************************ 

i 

i  prolog  interface  functions 

i 

',•••**<.»***••************<.*«********•»***•••*****•**««*,.* 

(dafun  at_tkm_limit 0 

(aand  asv  ; st-tkm-limit) ) 


(dafun  do_racovary ( ) 

(send  asv  :do-raoovary) ) 


(dafun  exacute.jslanned^otion  ( ) 

(aand  aav  sexeoute-planned-motion) ) 


(dafun  gr«phical_display (), 

(sand  asv  sgraphical-diaplay) ) 


(dafun  has_mora_tkm(lagl  leg2) 

(sand  aav  :has-more-tkm  lagl  lag2)) 
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{defun  inlts() 

(send  asv  sinitti) ) 


(defun  leg_with_new_foothold ( ) 

(send  eav  :  leg-with-naw-foothold) ) 


(defun  max^sm^leg (a-leg) 

(send  aav  imax-sm-leg  a-leg) ) 


(defun  modify_eommand< ) 

(send  asv  srnodify-corrmand) ) 


(defun  read_joystick () 

(send  asv  : read- joystick)) 


(defun  restore_command() 

(send  asv  ; restore-command) ) 


(defun  send<_deciaion(legl  leg2  a-deoision) 

(send  aav”: send-deoision  legl  leg2  a-decision) ) 


(defun  smalleat_tkin_leg() 

(send  asv  s  amalleat-tkm-leg) ) 


(defun  slow_down_robot () 

(send  asv  i slow-down-robot) ) 


(defun  apeed_up_robot () 

(send  aav  i speed-up-robot) ) 


(defun  ntable_p() 
(send  asv  : stable) ) 


i 

(defun  stable_p_m() 

(send  aov'istable  m) ) 


(defun  stable_without (a-leg) 

(send  asv  :stable-without  a-leg)) 
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(defun  update_robot_atatua O 
(aend  aav  :update”robot-atafus) ) 


I 


1 
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;;;  Mode  ;  Common-Lisp;  Package  ;USER,-  Base  :1c 

t 

i  top  level  motion  planning  coordinator 
/ 


(defun  my-monitor  ((rest  args) 

(let  ((x  (mapcac  #' my-output  ergs))) 

(if  (remove  nil  x) 

(my-print  x) ) ) 
t) 

(defun  my-output (arg) 

(oond  < (typep  arg  'leg)  (send  arg  :nam®)) 

((typep  arg  'atom)  arg) 

((typep  arg  'list)  (cons  (my-output  (oar  arg)) 

(my-output  (cdr  arg) ) ) ) 

(t  'error) ) ) 


/ 

; 

(defmaoro  retract (predicate  (optional  (argument  t)) 

Moond  ((not  (boundp  (quote  , predicate) ) ) 
nil) 

((and  predicate  (equal  , argument  '?)) 

(setf  , predicate  (cdr  , predicate)) 
t) 

((member  , argument  , predicate  :teat  'equal) 

(setf  (predicate  (remove  , argument  , predicate  soount  1) ) 
t) 

<t  nil))) 


(defmaoro  asaurta (predicate  (optional  (argument  t)) 

Moond  ((not  (boundp  (quote  , predicate) ) ) 

(setf  ,predicate  nil) 

(setf  ,predicate  (cons  , argument  (predicate))) 

(t  (setf  (predicate  (cona  , argument  (predicate))))) 


(defmaoro  aaaerci (predicate  (optional  (argument  t) ) 

Moonc  ((not  (boundp  (quote  (predicate))) 

(setf  (predicate  nil) 

(setf  (predicate  (append  (predicate  (list  (argument)))) 

(t  (aetf  (predicate  (append  (predicate  (list  (argument)))))) 


(defmacro  match (predic. .ite  (optional  (argument  t)  ) 
Moond  ((not  (boundp  (qgote  (predicate))) 
nil) 

((member  , argument  (predicate  :test  'equal 
t) 

(t  nil) ) ) 


(defmacro  unify (predicate  argument) 

Mcond  ((not  (boundp  (quote  (predicate))) 
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nil) 

(t  (setf  , argument  (car  /predicate))))) 


;  robot  initialize,  repeat,  my_loop,  fail. 

;  initialize  inita,  init_ditch_plan . 

i  init_dicth_plan  s-  retract (plan_cycle (_) > ,  retract (plan_state (_)) ,  fail. 

;  init”ditch_p.lan  asserts (plan^cycle  (T) ) ,  auserta <plan~state (place_legs_in_the_air) ) . 

;  my_loop  get^command,  plan,  execute,  !. 

t  get_command  X  is  read_joystic)c. 

;  plan  ditchjnode,  ditchjplan. 

;  plan  normaljplan. 

;  ditchjnode  t-  ditchjmode (in) .  n  oleared  by  ditch_plan. 

/  ditchjnode  t-  x  is  at^ditch^area,  x  —  t,  asserts (ditchjmode (in) )  . 

;  execute  executejnotion,  draw_robot,  !. 

;  exacute_moticn  X  is  executejplannedjnotion. 
l  draw_robot  s-  X  is  graphic  a  l_di  splay. 


(da fun  robot  0 
(create-terrain) 
( robot  1) 
(kill-terrain) ) 


(defun  robotl  0 
(initialize) 

(do  () 

( (not  (my_loop) ) ) ) ) 


(defun  initialize  0 
(cond  ((and  (inita) 

(init_ditch_plen) ) 
t) 

(t  nil))) 


(defun  init_ditch__plan  () 

tcond  ( (and  (not  (setf  ditchjnode  nil) ) 

(not  (setf  plan_cycle  nil) ) 

(not  (aetf  plan_atate  nil) ) 

(not  (setf  limit_leg  nil) ) 
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(not  (set f  reduce_spead  nil) ) 

(not  (setf  front_legs  nil)) 

(not  (setf  middle_logs  nil)) 

(not  (setf  rear_legs  nil) ) 

(not  (setf  decision  nil)) 

(asserts  plan_cycle  1) 

(asserts  plan~state  'place_legs_in_the_air) ) 


t) 

(t  nil))) 


(dofun  my__loop  ( ) 

(prooesa’-allow-sthedule) 
(oond  ( (and  (get__command) 
(plan) 
(execute) ) 


t) 

(t  nil))) 


(defun  get_ccmmand  () 

(oond  (t  (reed  joystick)) 

(t  nil) >7 


(defun  plan  () 

(oond  ( (and  (ditch_mode) 
<ditoh_plan) ) 
t) 

( (normal_jplan) 
t) 

(t  nil))) 


(defun  ditch_mode  0 

(oond  ( (match  ditch  mode  '  in) 
t) 

((and  (at_d.itch_area) 

(asserta  ditch_mode  '  in) ) 
t) 

(t  nil))) 


(defun  execute  () 

(cord  ((and  (ex<scute_motion) 
(draw_robot) ) 
t) 

(t  nil))) 

t 

(defun  execute_motion  () 

(cond  (t  (execute_planned  motion)  t) 
(t  nil))) 


(defun  draw_robot ( ) 

(cond  (t  "graphical  display)  t) 
(t  nil))) 
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i J *************** 

) ; 

ii  Normal  Plan 


!  i 

I  *********************************************************** 


;  normal  olan  i-  updata_robot_atata,  chaek_tkm_limit, 

;  Tegjplan,  bodyjplan,  ganarata^dacilionT  ! . 

;  updata-.robot_state  X  is  updata_robot_statua. 

/  cheok_tkmJLimit  A_leg  is  at_tkm_limit,  A^lag  \—  nil, 
i  ~  ~  assart  a  (limiO«flr7Ai_leg,  lift) ) . 

I  cheak_tkm_limit .  “  ™ 

;  lagjjlan  s-  li£t_aJLeg. 

>  lag_plan  t-  axchanga_laga . 

/  lag_plan  i-  atabla, 

/  lagjplan  s-  plaoe_a_leg. 

;  lagjplan  t-  wait^forJLaga . 

>  atabla  i-  Condition  is  stablajp,  Condition  •»  t. 

i  lift_.a_.lag  i-  atabla,  A_lag  ia  amaLLaat_tkm_lag,  A_lag  \»  nil, 

;  ""*  Condition  la  stable_*rithout7A_lag) ,  Condition  ■■  t, 

;  asaarta (decision <A_lag,_, liftT) . 

i  axchanga_lags  atabla,  LagA  is  smallaat_tkm_lag,  LayA  \—  nil, 
i  “  LegB  ia  max_am_lag (LagA) ,  LagB""\«»  nil, 

i  Condition  ia  has_moEe_tkm(LagB,  LagA) , 

;  Condition  *■*•  t, 

i  assarts (decision (LagA, LagB,axchanga))  , 

t  piac*._a_lag  s-  A_lag  is  m*x_am_  lag  (_) ,  A_lag  S--  nil, 

;  asaarta (daciaion (A_lag,_, placa) )  . 

i  vait_for_lags  try._naw_.£cothold. 

;  wait_for_lags  racovary,  asaarta <raduca_apaad) . 

;  wait_for_lags  s-  asaarta (raduoa_apaad) ,  rastora_limit_lag. 

;  try_naw__foothold  A_lag  ia  leg_with_new_foothold,  A_lag  \«"  nil, 
/  asaarta (decision (A_Iag, 7, placa) ) , 

;  racovary  A_lag  is  do_rocovary,  A_lag  nil, 

;  asaarta (decision (A_leg,_, place) ),  rastora_limit_l#g. 

;  rastora_limit_lag  ratract (limit_lag (A_lag,  lift) )  . 

;  rastore_limit_ieg. 


(defun  normal  jslan  () 

(cond  ((and  (update_robot_state) 
(chack  tkm  limit) 
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(leg_plan) 

<body_plan) 

(my-monitor  limit_leg  decision  reduce_apeed) 
(generate  decision) ) 
t) 

<t  nil))) 


(dafun  update_robot_state () 
(cond  ((update  robot  status) 
t) 

(t  nil))) 


(defun  check;  tkm  limit  <) 

(let  ( ( legT ) 

(cond  ( (eetf  leg  (et_tkm_limit) ) 

(asserts  limit*  leg  (list  'lift  leg)) 
t) 

<t  t) 

<t  nil)))) 


(defun  leg_plan() 

i  OR  tree  becomes  regular  "cond"  statement, 
(cond  < (lift_aJLeg)  t) 

< (exchange_leg)  t) 

((stable)  ”t) 

( (place_a_leg)  t) 

((wait  for  legs)  t) 

(t  ml))) 


(defun  stable  0 

(cond  ((stable_p)  t) 
(t  nil))) 


(defun  lift  a_leg() 

(let  ((leg)) 

(cond  ((and  (stable) 

(eetf  leg  (smallest_tkm_leg) ) 
<stable_without  leg) 

(asserts  decision  (list  'lift  leg))) 
t) 

(t  nil)))) 


(defun  exchanga_lag {) 

(let  ((legs)  (legfc)) 

(cond  ((and  (stable) 

(setf  lega  (*mallest_tkm_leg) ) 

(eetf  legb  (mux_«m_log  legs)) 

(has_more_tkm  legb  lega) 

(asserts  decision  (list  'exchange  lega  legb))) 
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(t  nil)))) 


(dafun  plaoa_a_lcg ( ) 

(lit  ((leg")" 

(cond  ( (and  (aatf  lag  '(max_am_leg  nil) ) 

(aaaarta  decision  (liat  'placa  lag))) 
t) 

(t  nil)))) 


(dafun  wait  for  laga() 

;  OR 

(cond  ( (tryjoew^fcothclda)  t) 

( (rocovary)  (aaaarta  daddoe^apead)  t) 

( (aaaarta  raduea  kpaad)  (rdstora  limits  lag)1  t) 
U  nil))) 


(dafun  try_.new  footholds () 

(lat  ((leg))" 

(cond  ((and  (aatf  lag  (lag  with  naw_foothold) ) 

(aoaerta  daoialonfTiat"' placa ’lag))) 
t> 

(t  nil))))., 

(dftfun  recovaryO 
fiat  (dag)) 

(cond  ((aatf  lag  (do_raoovary) ) 

(aaaarta  decision  (liat  'placa  lag)) 

( sa«tora_li,mitii.lag) 

t) 

(t  nil)))) 


(dafun  raatosa1 limit  lag() 

/OR 

(cond  ((and  (unify  limit^lag  lag) 

(ratract  limit  lag  lag)) 
t> 

<t  t) 

(t  nil) ) ) 


n 
n 
1 1 
1 1 
1 1 


***A*******«**t-***#**ft****#**********#*#tt***************** 

Ditch  Plan 


t  ditch_plan  ditch_plan,_don«,  ratract  (ditch_mode  (ir.) ) ,  idle_cycle 
;  ditch_plan  cyclejolannar .  ~ 


/ 

;**********  Cycle  planner  ************ 
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l  ditch_plan_dona  : -  plan_eyela ( 6) ,  ratract (plan_cycla ( S) ) ,  assarta(plan  eye  1* ( 1 >  >  r 
/  prapara__naxt_ditch_plan. 

;  prapara_naxt_ditchjplan  mova. 

i  cycla_plann#r  onaj:ynla_dona,  plan_cyola (N) ,  N1  ia  N+l,  ratract (plan  cycla(N)),  as: 
ta (plan_cycla !N1) ) ,  “ 

;  ~  idla_cycle . 

/  cyolajplannar  plan“cyola. 


; **********  plan  Cycla  diapatohar  ************ 

/ 

;  ona^cyelajiona  i-  plan_atata (onajpltn_eyela_dona) ,  ratraet(plan  atato  (onajplan  eyola  d 
a)>, 

1  initialixa_plan_atata . 


;  plan  oyola 
_daeiaTon, t . 
i  plan  oyola 
_daciaion, ! . 
;  plan cycla 
_daoiiTon, I . 
!  plan  oyola 
^daciaTon, I . 
t  plan_oyola 
_d«Gi»ion, I , 


plan_cyola (1) ,  updafea_robot_tfcato,  ditoh_plan_oycla_l,  bodyjplan,  ganara 
plan_cycla (2) ,  updata_robot_»tata,  ditch_plan_cycla_2,  bodyjplan,  ganara 
plan_cyola 0) ,  updata_robot_#tata,  ditchjplan_eyela_3,  body_plan,  ganara 
plan_cyala (4) ,  updata_robot_atata,  ditohjplanjjyolajl,  bodyjplan,  ganara 
plan_oyola (3) ,  updata_robot-(atata,  ditohjplan_oycla_S,  bodyjplan,  ganara' 


i  idla^cyola 


updata_roL)Ot_atata,  bodyjplan,  ganarata_daaieion,  ! , 


(daCun  ditoh_pl«n  () 

(cond  ((and  (ditoh_plan_dona) 

( rat r act  ditch_moda  'in) 
(idla_oyola) ) 


( (cyola_plannar) 
t) 

(t  nil))) 


/ 

/**********  Cycla  planner  ************ 

i 


(dafun  ditchjplan_dona  0 
(cond  ((and  (match  plan_cyola  6) 

(ratraot  plan_cyola  6) 
(assarta  plan_oyole  1) 
(prapara_naxt~ditchjplan) > 


t) 

<t  nil))) 


(def  un  prapara_naxt_ditoh_plan  () 
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(cond  ( (mova) 
t) 

(t  nil))) 


(dafun  cyclajplannar  () 

(cond  ((and  (onA^eyela^donA) 

(unily  plan_cyola  N) 

( rat r act  plan_cycla  N) 
(aaaorta  plan”cycla  (+  N  1) ) 
(idlA_cyclA) ) 


( (plan^cynla) 
t) 

(t  nil))) 


; 

^ ********* *  Plm  Cycle  Dispatcher  ************ 

; 


(dafun  ona_aycla_dona  () 

(cond  ((and  (matoh  plan_atata  '  onajplan_oyala_dona) 

( rat r act  plan  atata  ' ona__plan_cycIa_dona) 
(initialiaajplan^atata) )  “ 


t> 

<t  nil))) 


(dafun  plan_oyela  <) 

(cond  ( (and  (match  plan_cycla  1) 

(updata_robot_atata) 

(ditahjplan_oycla_l) 

<body_plan)”" 

(my-monitor  plan_cycla  plan_stata  daoiaion  radua*_spaad) 
(panorata  daoiaion) ) 
t) 

( (and  (match  plan_oycla  2) 

(updata_cobot_atata) 

(ditch  jplan_cyola_2) 

(bodyjplan)**  "** 

(my-monitoe  plan_oycla  plan_atata  daoiaion  raduca^apaad) 
(ganarata  daoiaion)) 
t> 

( (and  (matoh  plan_oycla  3) 

(updata_robot_atata) 

(ditohjplan_cyola_3) 

(bodyjplan) 

(my-monitor  plan_oyola  plan_stata  daciaion  raduca_spaad) 
(ganarata_daoision) ) 


t) 

((and  (matoh  plan_pycla  4) 

(updata_robot_atata) 

(ditoh_jplan_oyola_4) 

(bodyjplan )- 

(iny-monitor  plan__cycla  plan_atata  daciaion  reduoa_spa8d) 
(ganarata_daoiaion) ) 


t) 

( (and  (match  planjcycla  3) 
(updata_robct_atata) 
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t) 

(t 


(ditch_plan_cycle_5 ) 

(body_jjlan)  ~ 

(my-mo.nitor  plan_cycle  plan_atate  decision  reduce_speed) 
(generate_deciaion) ) 

nil))) 


(defun  idle_oyole  0 

(oond  ( (end  <update_robot_atate) 

(body_plan) 

(my-monitor  plan_cyole  plan_atate  deoiaion  reduce  speed) 
(g*nerate_decision) ) 
t) 

(t  nil))) 


i 

^**********  oyclti  ********* 

; 


l  initialisejplan_atate  t-  asserts (pl»n_*tete (atart) ) . 


I  ditahjplan.ayale.,1  i  - 
plaoe_leffa_in_the_air) ) 


ditohjplan_cycle_l  i- 
ditahjlan_cycle”l  i  - 
ditoh_plan“eyele“l  i- 
ditohjplan_oyale”l 
ditohjplan_ayole_l  i - 
ditch_plan~cyale”l  t- 


plan_atate (atart) ,  retraot;  (plan_atate (start) ) ,  esaerta (plan_atat 

placa_leg«_in_6he_air  (back_middle_legs>  . 
placa_lega_in_the_air (baak_middle“lega) . 
baok_middle_legs (forward. rear  lege) , 
forward<_raar_lega (forward_mid3ie_lega) , 
fiorward_middla_lega ( forward. front_lega) . 
forward_f ront_lega <lift_middle_lega_and_move) , 
lift_uviddie_legs_and_move (one_j>lan_oyole_done)  , 


/  ditohjplan_oyole_2 
baok_middle_lega) ) , 

* 

i  ditch_plan_oycle_2 
;  ditohjplan~cyale„2 
i  ditah_plan_ayole_2 


»“  plan_state (start) ,  retract <plan_state (start) ) ,  asserta (plan_stat 

back-middle_lega (forward_rear_legs) , 
back_middle_legs (forward~rear~lags) . 
i-  forward_rear_legs <forward_middle_legs)  . 
i-  forward_middle_lega  (one_pTan_oyoTe.done)  . 


;  ditch_plan_cycl<a_3  i-  plan_atate (start) ,  retract  (plan  state  (atart) ) ,  asserta  (plan  stat 
move_fotward_front_legs) ) ,  ” 

i  move_forward_f ront_lege (mcve_forward_middle_lega) . 

;  ditoh_plan._cycle_3  move_forward— f ront_legs  (move_back_middle._lega)  . 

i  ditahjplan_cycle_3  i-  move_baok_middle_>legs  (move_.forward_rear_lega)  . 

i  ditoh_plan_cyole_3  move_forward_rear_legs (one_plan_cycle_done) . 


;  ditchjplan_cycle_4  plan_state (start) ,  retract <plan_atate (start) ) ,  asserts (plan  atat 
move_forward_middle_ legs) ) ,  ” 

i  move_forward_middle_lega (one_plan_cycle_done) . 

/  ditch_plan_cycle_4  move_forward_middlei.legs  (one_plan_cycle_done)  . 


i  ditch__plan_cycle_5  plan_atate (start) ,  retract  (plan__state  (start) ) ,  asserta (plan_stat 
move_forward_£ront_lega) ) ,  ~ 
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I  mova_forward_ironc_legs  (movs_f  orward_middle_legs)  . 

i  ditch_plan_cycla_S  j-  move”fiorward_front_legs  (move_backjmiddla_legs)  . 

/  ditch_pl«n”cycle3  •  "  mova~baek_middla_laga  imove_forward..rear_ilegs)  . 
i  ditcl'._pian”cycle”5  i-  move~forward_rcar_legs  (one_plan_cycle_done)  . 


I 

l 

l 


Cycles  ****************** 


(dafun  initialise _plan_stata  0 
(cond  ( (assart*  plan  atata  'start) 
t) 

<t  nil))) 


(dafun  ditch _plan_cycle_l  0 

(cond  ((and  (match  plan_state  'start) 

(ratract  plan_atata  'start) 

(asaarta  plan~state  'placa_lags_in_tha_air) 
(placa_lags_.in_tha_.air  'back_middle_legs) ) 
t) 

<  (plaoa  laga  in  tiha  air  'back  middla  lags) 
t) 

((back  middla  laga  'forward  raar  laga) 
t) 

((forward  raar  lags  'forward  middla  lags) 
t) 

((forward  middla_lags  '  forward  front  lags) 

t) 

((forward  front  lags  'li£tjtiiddla_lags_and  mova) 
t) 

((lift  middla  lags  and  mova  'onajplan  cyola  dona) 
t) 

(t  nil))) 


(dafun  ditah_plan_oycla_2 ( ) 

(cond  ((and  (matoh  plan_atata  'start) 

(ratract  p!an_state  'start) 

(assarts  plan~stata  'baok_middla_lags) 
(back  middla  lags  'forward  raar  lags)) 
t ) 

( (baok  middla  lags  'forward  raar  lags) 
t) 

((forward  raar  lags  'forward  middla  lags) 
t) 

((forward  middla  lags  'ona _plan  eyola  done) 
t) 

(t  nil))) 


(dafun  ditah_plan_cycle_3  0 

(cond  ((and  (matoh  plan_ststa  'start) 

(ratract  plan_state  'start) 

(assarta  plan~3tate  'move_forward_front_legs) 

‘  (move  forward  front  lags  'move  baok  middle  legs) ) 
t) 

( (move_forward_f ront_lcga  'move_back_middla_lags) 
t) 

((move  back  middla  lags  'move  forward_rear  lags) 
t) 

( (move_forward_raar_lags  'ona _pian_cycle_dona) 
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t) 

(t  nil))) 


(dafun  ditohjplan_cyole_4  () 

(cond  ((and  (match  plan_state  'start) 

(retract  plan^state  'start) 

(assarts  plan~state  'move^forward^mi.ddla^lags) 
(move_£orward_middla_legs'’"'  one_plan_eyole_done) ) 


( (rrova  forward  middle  legs  'one_plan  cycle  done) 
t) 

(t  nil))) 


(defun  ditch_plan_cyale_5  <) 

(cond  ((and  (match  plan_state  'start) 

(retract  plan_state  'start) 

(asserts  plan”atata  'move_fOEward_front_legs) 
(move  forward  front  legs  ~mova  back  middle  legs)) 
t) 

((move  forward  front  lags  'move  back  middle  lags) 
t>  ”  ” 

((move  back  middle  legs  'move  forward  rear  lags) 
t) 

((move  forward  rear  legs  'onejplan  oyola  dona) 
t) 

(t  nil))) 


; 

; ************ **  states  *************** 

; 


i  back_middla_iogs (Naxt_State)  plan_state (back_middle_legs) ,  back_middle_lags  dona, 
i  retract (plan_state (backjniddle  legs)),  assertaTplan  st 

a (Next_Stata) ) , 

i  stop. 

;  back_middle_lega (Next_State)  plan_state (back_middla_legs) ,  do_back_middle_lega, 
i  stop.  ~ 


t  forward_f ront_legs <Naxt_Stata)  plan  state  (forward_f ront  lags)),  forward_f ront  legs 

na, 

retract (plan_state (forward  front  legs),  assarta(plan 
tata (Next_State) ) ,  “ 

t  i  stop. 

;  forward_f ront_legs (Naxt_Stata)  plan_stato  (forward_f ront_lags) ) ,  do_forward_f ront_la 

t 

<  stop. 


;  forward_ middle_legs (Next_State)  plan_state  (fotward_middle_legs ) ,  £orward_middla_l«g 
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done, 

;  retract (plan_atate ( f orward_middle_lega ) ,  aaaesta(p‘ 

_state (Next_state) ) ,  ~ 

;  stop, 

/  £orward_middle_legs (Next_State)  i-  plan_atate (£orward_middle_lega) ,  do_£orward_middle, 
gs, 

;  atop. 


/  forward.jrear^lega  (Next—3tate)  i-  plan_atate (forward^rear^lega) ,  £orwaEd_rear_legsjion« 
;  “  “  ~  retract (plan_atate7forward_r#ar_lega) ,“aasarta (pTan_e 

te  (Next_State) ) ,  ~  ” 

;  ~  atop. 

;  £orward_rear_legs (Next_State)  »-  plan^state (£orward_rear_lega) ,  do_£orward_rear_lega, 

;  ~  ”  atop.  ” 


/  Hft_middl(i_lege_andjnove  (Naxt_3tate)  i-  plan_atate (lifit_middle_lega_and_mova) ,  move_c 
e,  atop, 

l  retraot  (plan_stata  (li£t_middle_leg*_and_>move) 

asaerta(plan_atato<Next_State) ) .  ~  ” 

l  li£t_middle~laga_and_move  (Naxt_3tata)  t-  plan_atate  (liftjmiddle_lega<<.arid_niove) ,  do^lif 
middl*~lega,  move.""  ~  **  ‘  ~  ”  "" 

/ 


l  mova_baak_middle_lega (Naxt_St*te) 
_lega_done,~ 

/ 

(plan_at«te (Next_8tete) ) . 

/  move_bac'c_middTe_lega  (Next_State) 
dlejlega .  ” 


t-  plan_etate  (move_back_middle_lega) ,  move_baok_midc 
retraot  (plan_state  (move_baok_middl.eJlegs)  > ,  asser 
i-  plan_atate  (move.Jjackjmiddle^legrs)  ,  dojnoveJoaekjr 


;  move^forward^front^lega  (Next  ,3r.atej  s-  plan^atate (move_£orward_£ront_lega) ,  move^fiorwa 
_£ront”lega_done,  ”  ” 

i  retraot (plan_atate (move_fiorward_f ront_lega> ) ,  a 

erta (plan_state (Next_Stata) ) .  " 

/  move_£orward..£ront_lega (Next_State)  i-  plan_atate (move_forward_f ront_lega) ,  do_move_fc 
a  rd^f  r  ont_Iego\" 


;  move  t'orward_mi.ddle_legs (Next_State>  s -  plan_state (move_forward_middle_lega) ,  move_£or 
rd_midale_lega_done, 

/  retraot (plan_atate (move_forward_f ront_lega) ) , 

aerta (plan_atete (Next_State) ) . 

t  move_.£orward_middle_lega (Next_State)  !•■  plan_state (move_fiorward_middle_lags) ,  do_move_ 
rward_middle_lega .  ” 


;  move_£orward_rear_lega <Next_State)  pla»_atate (move_£oEwerd_Eear_lags) ,  movo_forwarc 
iddle_l ogs_done , 

;  retract (plan_state (move_£orward_-ear_iegs) ) ,  as t 

ta (plan_atate (Next_8tate) ) . 

/  move_f ocward_rea7_lega (Next_Stata)  plan_atate (move_£orward_roar_lega) ,  do_move_£orv. 

d_rear_lega . 
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i  place_leg9_in_tha_air  (Naxt_State) 
e_*ir_dona, 

> 

(plan_stata (Naxt_atata) ) , 

; 

i  plaeai_lags_in_tha_«ir (Naxt_$tata) 
_tha_air,  atop. 


plan_stata (placa_lega_in_the_air) ,  place_laga_in_ 
ratract (plan^atata (piase_legs_in_tha_eir) > ,  aaaer 
atop. 

plan_.atata  (pl«ca_laga_in_tha><air) ,  dojplaca^lags^ 


t 

f ************* *  St®tos  *  * * ******** * ** * 

; 


(dafun  baok_middla_laga  (naxt_atata) 

(oond  ((and  (match  plan_stata  'back_middla_laga) 
(baok_tniddla_laga^dona) 

(ratraot  plan_atata  'baek_middla>_lags) 
(aaaarta  plan~atata  naxt_atata) 

(stop) ) 
t) 

((and  (match  plan_stata  ' baok_middla_lags ) 
(do_baok_middla__laga)  * 

(atop) ) 
t) 

(t  nil))) 


(dafun  fOEward_£ront_laga  (naxt_atata) 

(oond  ((and  (matah""plan_atata""' forward_front_laga) 
(forward_front_lags_dona)  ~ 
(ratract~plan_atata  ' forward_front_laga) 
(assarta  plan_atata  naxt_stata) 

(atop) ) 
t) 

((and  (match  plan_atata  ' f orward_f ront_laga) 
(do_fotward_f ront_laga) 

(atop) ) 
t) 

(t  nil) )  ) 


(dafun  forward_middla_lags  (next_stata) 

(cond  ((and  (match  plan_atata  ~forwacd_middla_lags) 

(f orwacd_middla_laga_dona) 
(catract""plap_atata  ”foxwaed_n\iddla_laga) 
(aaaarta  plan_atata  n*xt_atata) 

•  (stop)) 
t) 

((and  (matoh  plan_atata  ' forwerd_middla_laga ) 
(do_forward_middla_lags) 

(atop) ) 
t) 

(t  nil) ) ) 
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(dafun  forward_rear_lega  (naxt^stata) 

(cond  ((and  (match  plan_atata  ' forward_raar_legs) 
(£orward_raar_lega__dona) 

(retract- plan~atata  ' forward_r«ar_leg3) 
(aaaarta  plan-atata  naxt_atate) 

(atop) ) 
t) 

((and  (match  plan_atata  ' forward_raar_legs) 
(do_forward-raar_laga) 

(atop)) 

t) 

(t  nil)) > 


(da fun  lift_middla_laga_and_mova  (naxt_$tata) 

(ootid  ((and  (match  plan_atate  ' liftjniddle_lags_and_move) 
(mova_don«) 

(atopT 

(ratraot  plan_atata  ' lift_middla_laga„and_mova) 
(aaaarta  plan“atata  naxt_atata) )  ™ 

t) 

((and  (matoh  plan_atata  ' lift_middle_lege_and_move) 
(lift_middla_lega)  ~ 

(movaT ) 
t) 

<t  nil))) 


(dafun  move_back_ndddle_leg»  (naxt_atata) 

(cond  ((and  (matoh  plan_atate  'mova_baok_middl8_laga) 
(mova_baok_middla_laga_done) 

(ratraot  plan_atata  'mova_baok_middla_.laga) 
(aaaarta  plan  atata  naxt  atata) ) 
t) 

((and  (matoh  plan_stata  'mova_back_middla_laga) 
(do  mova  back  middle  lag.')  ) 
t) 

(t  nil))) 


(dafun  mova_forward_front_laga  (next_state) 

(cond  ((and  (match  plan”ftata  'mova_forward_£ront_lag3) 
(mova_forwart_front_laga_dona) 

-  (ratraot  plan~stata  'mova_forward_front_laga) 
(aaaarta  plan_atata  naxt  atata) ) 
t) 

((and  (matoh  plan_state  ' mova_forward_f ront_.l eg?) 

(do  mova  forward  front  laga)) 
t) 

(t  nil) ) ) 
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(defun  move_forward_middi«_leg3  (next_8tate) 

(cond  ((and  (match  planjstate  'move_forward_middle_lega) 
(move_forward_middle_legs_done) 

(retract  plan_state  'move_fosward_middle_lega) 
(asaerta  plan_state  next_atate) ) 
t) 

((and  (match  plan_statu  'move_forward_middle_lega) 
(do  move  fOEward_middie__legs) ) 
t) 

<t  nil))) 


(defun  move  forward  rear  legs  (next_atate) 

(cond  ((and  (match  plan_atate  'move_forward„rear_lega) 
(move  forward_rear_lega_done) 

(retract  plan_itate  'move_forward_rear_lega) 
(asaerta  plan~atate  next  state) ) 
t) 

((and  (match  planjstate  'move_forward_rear_legs) 
(do  move  forward  rear  legs) ) 
t) 

<t  nil))) 


(defun  place_legaii>iin_the<.air  (next^atate) 

(cond  ((and  (matoh  plan_atate  '  place  jlegajLn..thejiir) 
<plaoe„lega_in_the_air_>done) 

(retract  plan_state  ' place_iegs_in__the_air) 
(asaerta  plan“atate  next_state) 

(atop) ) 
t> 

((and  (match  plan_atate  'place_lega_in_the_air) 
(do_j)lace_lega_in_the_air) 

(stop) ) 
t) 

(t  nil) ) ) 


/ 

;  ****************  State  Executors  ******************** 


i  move_back  middle  legs  done  all_middle_legs_lifted,  all_middJ.e_lega_placed, 
i  ~  clear.middle_lifted_memory,  c lea r_move^ memory,  atop. 

i  do__move_bafik_middle_legs  all_midd.le_legs_lifted,  move_done,  stop,  place_middle_leg 

ack . 

;  do_move_back_middle_loga  all_middle_legs_lifted,  move. 

;  do_mo ve_bank_middle_legs  lift_middle_legar  stop. 
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;  mov®_forward_f ront_legs_done  all_f ront_legs_lifted,  all_f ront_legs_placed, 

;  clear_£rent_li£ted_memory,  clear_move_Tnemory,  stop. 

t  do_mov«_forward_f ront_legs  all_f ront_l®gs_lifted,  move_done,  stop,  place_f ront_legs 
;  dojmova^forward'f ront~legs  i-  all~f ront~legs~li£ted,  move. 

;  do~mov»2fo*ward_front_l«9S  li£t_£ront_l®gs,  stop. 


;  mov«„£orward_middle_l«gs.,dona  all_middla_legs_li.fted,  all_middl®iJleg3-placed, 
t  ™  -  -  c lea r_middle_l i?ted_memo ry ,  ~c  i®  a r_mov«_m®mo r  y ,  stop. 

;  do_n\ove_£orward_rrdddla_legs  all_middle_legs_lifted,  move_done,  stop,  place_middle_l 
s . 

/  do_mov«_£oE»»ard_middla_l«g3  all_middle>_l*ga_li£ted,  mov® . 

/  do_move~£orward_rr,iddle_legs  i-  li£t_middle_legs,  atop. 


;  mov®_forward_roar_l®ga  don®  all_r®arJ.ogs_li£t®d,  all_r®ar_l®gsjpl®c®d, 

;  -  t  ol®ar_r®ar_li£t®d_m®mory,  ol®ar_ move_memory,  stop. 

/  do_mov®__forward_rear_legs  all_r®ar_i®ga_li£t®d,  mov®_don®,  stop,  pl*ce_rear_l®gs . 

;  do^move^forward'rtiaO-ags  all“c®at~l®gs“li£t®d,  mov®7 

/  do~mo v®~f o r wa rd_r® a r_l®gs  i -  li£t_r®ar_I®gs,  stop. 


>  mov®  i-  ass«rta (r®sum®jmov®m®nt) . 
/  stop  assarts (stopjmovement) . 


;  cl®ar_mov®_m®mory  r®tract  (mov®  (don®) )  . 
;  c 1®  a r_mo  vo_m*mo  r y . 


/  move_don® 
;  move_don« 
;  mov®  done 


iuove(don®)  . 

X  is  at_tkm_limit,  X  nil,  asserts (move (done) ) . 

X  is  at_stability_limit,  X  \«  nil,  asserts (move (done) ) . 


t  back_middl®_l®gs_dorno  all_middl®_leg8_lift®d,  all_middle_legsjplaoed,  clear_middle_ 
ft®H_memory,  cl®ar_mov®_m®mory . 

» 

;  do_back_middl®_legs  all_middl®_legs_li£t®d,  place_middle_legs_back . 

;  do_back_middl®_l®gs  li£t_middl®_l®ga . 

; 

;  all_middl®_l®ga_lifted  . -  middi®_l®ga (lifted) . 

t  all_middle_legs_lift«d  X  is  both_middle_legs_lifted,  X  --  t,  asserts (middle_legs (li 
ed)).  “  ”  , 

* 

i  all_middle_l®gs_plac®d  X  is  both_nJ.ddle_legs_placed,  X  —  t. 

i 

i  clear_middle_lifted_memory  retract (middle^lags (lifted) ) . 

i 

;  place_nuddls_legs_back  A_leg  is  plaoable_middle_leg,  A_leg  \—  nil,  asserts (decisio 
;i_l''g,_,place_back)  )  . 

;  place_middle_legs__baok , 


robot444 . lisp 


Thu  Nov  29  11:34:42  1990 


17 


/ 

!  lift_middle_legs  A  leg  1j  liftable  middle  leg,  A  leg  \-«  nil,  asserta  (decision (A  le 
_,lift)).  “ 

;  lift_middle_iegs . 

; 


;  forward^front^legs^done  all_f ront_legs_li£ted,  all_f ront^egs^jslaced,  olear_front  1 
t*d_memory,  clear_move_memory .  —  —  “  "" 


;  do_forward_f ront_lega 
/  do_£orward”fronO-egs 
/ 

i  *il_£ront_lega__liJted 
i  all  front  legs~li£ted 
). 


all_£ ront_legs_li£ted,  place_f  ront_lego . 
lift_f ront_legs . 

front_legs (lifted) . 

X  is  both_front_.legsJI.if  ted,  X  —  t,  asserta (frontjLegs (lifte 


/ 

;  all_front_lega_placed  X  is  both_£ront_legs_plaeed,  X  --  t. 


;  clear_frontJLifted_memory  i-  retract (front  legs (lifted) ) , 

;  ~ 

/  plaoe_£rontJ.ega  s-  AjLeg  is  plaoable_front_leg,  A  leg  \—  nil,  asserts  (decision  (A  leg 
, place) )  . 

/  place_front_legs . 

i  ~  “ 

t  lift_front_legs  A_leg  is  liftable_front_leg,  A  log  \—  nil,  asserta (decision (A  leg, 
lift)) .  “  ’ 

;  lift_£ront_legs. 


/  f o rwa r d_mi ddle_legs_done  !-  all_middle_legs_li£ted,  alljmiddle  lags_placed,  clear  midd. 
_lifted_memory,  c le  a  r_mo vejmamo  r y .  “  "  “ 

; 

;  do_f  orwa rd_middle_lega  !-  all_middle  legs  lifted,  place_middle_legs . 

/  do_forward_middle_lego  lif t_middle_legs . 

i 

t  place_middle_lega  A_leg  is  placable  middle_leq,  A  leg  nil,  asserts (decision (A  1 
,.  , place) ) .  -  “ 

;  place_middle_lags . 


>  forward_raar_lags_done  all_rear_legs_lifted,  all_rear_legs_placed,  clear_rear  lifte 

memory,  clear_move_memory .  '  “  “ 

/ 

i  do_forward_rear_legs  all_f ront_legs_lifted,  place  rear  legs. 

;  do _forward_raar_legs  lift_rear~legs7 

i  all_rear_legs_li£ted  rear_legs (lifted) . 

‘  all_rear_legsJLifted  X  is“both_rear_legs_lifted,  X  «  t,  assorts (rear  lags (lifted) ) 

! 

I  ell_rear_lega _placed  i  is  both_rear_legs_placed,  X  t. 

;  clear_rear_lifted_memory  retract (rear_legs (lifted) ) . 

;  place_rear_legs  A_leg  is  plscable_rear_leg,  A_leg  \"  nil,  asserta (decision (A  leg, 
lace) )  .  -  -  -  -  “ 

;  place_rear_legs . 

>  lift_rear _legs  A_leg  is  liftable_rear_leg,  A_leg  \—  nil,  asserta (decision (A_leg,_, 
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ft)  )  . 

;  lift_r#ar_legs . 

; 


i  do_lift_middla_lags  : -  lift_middla_lsgs . 

9 

I 


I  plaoeJLegs_in>J:he_air_don*  j-  X  is  all_laga jplacsd,  X  t. 

t  placa_legs  : -  A_leg  is  placable_l#g,  A_lag  \—  nil, 

;  “  assart* (decision (A_leg,_, place) ) . 

/  place_legs. 


i 

^ ****************  StAti#  Executors  **************  *  ****  * 

; 


(dafun  mova^baclc^middl'a^lags^dona  () 

(cond  ( (and  (all_middle_lega_liftad; 

(all_ndddle_lega  ^placed) 

(o  Isa  E_middla_liftedjman\ory) 
( o 1 # a  x~mo va_msmo  r y )  ~ 
(stop)” 
t) 

(t  nil))) 


(dafun  do_move_back_middle_legs  0 
(cond  ( (and  (alljniddle^egs^liftad) 
(move_done) 

(placa_middl<*_legs_bac)c) 
(stop) ) 
t) 

( (and  (all_miUdle_legs_lifted) 
(move) ) 
t) 

( (and  (lift_jniddl*_l*gs) 
(stop!) 

t> 

(t  nil))) 


(dafun  movs_fcrward _f ront_legs_done  () 
(cond  ((and  (all_front_leg3_liftad) 
(sll_£ront_lags_placed) 
(cl«ar_f.ront_lift«d_memory) 
(clsar_movs_memory) 
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(stop) ) 
t) 

(t  nil))) 


(deiun  do_move_£orw*rd_f ront_legs  () 
(cond  ( (and  7all_£ront_l®gs_lifted) 
<move_done) 

( pi ace_f  r  ont_lsgs ) 
(stop) 7 

t> 

( (and  (all_front_leg7._lifted) 
(move) ) 
t) 

((and  (lift^frontjlags) 

(stop 7) 
t) 

<t  nil))) 


(defun  move_forward_middle_lags_done  () 
(cond  ((and  (allJniddl«_legs_Tift«d) 
(all_middl*_legs7plaaed) 
(aleir_middle_lifted_memory) 
(ales  r_mo  vs_jremory) 

(stop) 7 

t) 

(t  nil))) 


(defun  do_move_forwerdjmiddle_lege  () 
(cond  (Tand  7all_middle_legs_lifted) 
(mo”e_done)_ 
(place_middle_legs ) 

(stop) ) 
t) 

( (and  (all_middls_l«gs_<ilift«d) 
(mo vs) ) 
t) 

((and  (lift_middle_lega) 

(stop) ) 
t) 

<t  nil))) 


(defun  move_f orw*rd_rsar_legs_done  () 
(cond  ( (and  (all_rear_Tegs_Iifted) 
(all_rear_legs_placad) 

-  <olear_reai:_l<.fted_memory) 
( clea  r_mova_memo  ry ) 

(stop) 7 

t) 

<t  nil))) 
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(defun  do<jnove_forward_rear_lega  <) 
(cond  <”and  ~all_rear_lega_lifted) 
(move_done) 

(place_rea  r_lega ) 
(stop)T 
t) 

( (and  (all_rear_lega_li£ted) 
(move) ) 
t) 

( (and  (lift_reariilegs) 
(atop”) 
t) 

(t  nil))) 


(defun  move  () 

(cond  ((aaaerta  resume  movement) 
t) 

<t  nil))) 


(defun  stop  () 

(cond  ((asserts  stop  movement) 
t) 

<t  nil))) 


(defun  o le a r_mo ve_memo ry  () 
(oond  ((retract  move  'done) 
t) 

(t  t) 

(t  nil) ) ) 


(defun  move_done  0 

(cond  ((match  move  'done) 
t) 

( (and  (at_t)tm_limit) 

(aaaerta  move  'done)) 
t) 

((and  (at_atability_limit) 
(aaaerta  move  'done)) 
t) 

(t  nil))) 


(defun  back_middle_legs_done  () 

(cond  ((and  (all_middle_lege_lifted) 
(all”middle”legajalaced) 

(c lear_middle_l i f ted_memo ry ) 
(clear_move_memory) ) 
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t) 

(t  nil) )) 


(defun  do_back_middle_legs  <) 

(cond  (Tend  7all_middle_lega_lifted) 
(place  middle  legs  back) ) 
t) 

((lift  middle  legs) 
t) 

(t  nil))) 


(defun  all'jniddle  legs_lifted  () 

(cond  ((match  middle  legs  'lifted) 
t) 

((and  <bothjniddle_lega_li£t#d) 

(asserts  middle  legs  'lifted)) 
t) 

(t  nil) ) ) 


(defun  all  middle  legs jplaaed  () 
(cond  ( (both  middle^legs^laced) 
t) 

(t  nil))) 


(defun  clear_middle  liftedjnemory  () 
(cond  ((retreat  middle  legs  'lifted) 
t) 

(t  nil))) 


(defun  plaae^middle^legs^baak  <) 

(let  (leg)- 

(cond  ((and  (eetf  leg  (plaeable_middlei_leg> ) 

(asserts  decision  (list  'place  back  leg))) 
t) 

(t  t> 

(t  nil)))) 


(defun  lift_middle_legs  () 

(let  (leg) 

(cond  ((and  (eetf  leg  <liftable_middle  leg)) 

(aaaerta  decision  (list  'lift  leg))) 
t) 

(t  t) 

(t  nil)  ) ) ) 


(Uefun  forwatd_f ront_lega_jiona  () 

(cond  ((and  Talljf ront_Tegs_lift*d) 
(all_£ront_legs  jolaced) 
(oleir_f ront_lifted_memory) 
(clear_mov«  memory) 7 
t) 

(t  nil) ) ) 
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(dafun  do.forward_fzont.lags  () 

(cand  (Tend  (all.front.lags.lifited) 
(place  front.lags) ) 
t) 

((lift  front  lags) 
t) 

(t  nil))) 


(dafun  all_front.laga.lif tad  () 

(cond  ( (mat oh  front  lags  'liftad) 
t) 

( (and  (both_f ront_laga_liftad) 

(assarts  front  lags  'liftad)) 
t) 

(t  nil))) 


(dafun  all_front_lags._plaoad  0 
(oond  ( (both  front_laga_plaead) 
t) 

(t  nil))) 


(dafun  claar_front_liftad_mamory  () 
(oond  ((ratraot  front.lags  'liftad) 
t) 

(t  nil))) 


(dafun  plaea.front.laga  <) 

(lot  (lag) 

(oond  ((and  (aatf  lag  (plaoabla_.front.lag)) 

(assarts  daoision  (list  'plaoa  lag))) 
t) 

(t  t) 

(t  nil)))) 


(dafun  lift.f ront_lags  0 
(lat  (lag! 

(oond  ((and  (aatf  lag  (liftsbla.front_.lag)) 

(assarts  dacision  (list  'lift  lag))) 
t) 

(t  t> 

<t  nil)))) 


(dafun  forward_middla.lsgs.dona  () 

(cond  ((and  (all.middla.lags.liftad) 
(all_middla_lags_plaaad) 

( o lasr  jmiddla.l i f  t od.mamo  r y ) 
(alaar"~mova  mamory))”" 
t) 

(t  nil))) 
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(defun  do_fotward_middle_legs  () 

(oond  ( (end  (all_middle_lega_lifted) 
(place_middle_legs) ) 
t) 

((lift  middle  legs) 
t) 

(t  nil) ) ) 


(defun  place_middle_lega  () 

(let  (leg) 

(oond  ((and  (aetf  leg  (plncablo>jniddle-leg) ) 

(aaaerta  decision  (liat  'plaoe  leg))) 
t) 

(t  t) 

(t  nil)))) 


(defun  forward_rear_lega_done  0 
(oond  ( (and  T*ll_reer_Iega_lifted) 
<all_tear_legajplaeed) 
(oleaE_Eear_lifted_m«mory) 
(□lea  r-mo  ve-memo  r  y ) ) 
t) 

<t  nil)) ) 


(defun  do_forward<iirear_lega  <) 

(oond  ( (and  (all_rear_laga_lifted) 
(plaoe  rear  lega)) 
t) 

( (lift_rear_lega) 
t) 

(t  nil) )  ) 


(defun  all_reer>JLega-jLifted  () 

(oond  ( (match~‘rear“legs  'lifted) 
t) 

( (and  (botn_rear_lega_lifted) 

(aaaerta  rear  lega  'lifted)) 
t) 

(t  nil)) ) 


(defun  all_rear_legajplacad  () 
(oond  ( (both_rear_lega_plaoed) 

t)  ( 

(t  nil))) 


(defun  olear_rear_lifted_m«mory  () 
(oond  ((retreat  rear  lega  'lifted) 
t) 

(t  nil) ) ) 
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(defun  place_rear_legs  () 

(let  (leg)“ 

(oond  ((and  (s«t£  lag  (placable_raar_leg) ) 

(assarta  decision  (list  'place  lag))) 
t) 

(t  t) 

(t  nil)))) 


(dafun  lif t_rear_legs  () 

(let  <leg“ 

(cond  ((and  (aatf  lag  (li£tabla_raar_lag) ) 

(assarta  decision  (list  ' li£t  lag))) 
t) 

(t  t) 

(t  nil)))) 


(dafun  do_lift_middla_lega  () 
(cond  (Tli£t_middla_lags) 
t) 

(t  nil))) 


(dafun  place_legs_in_the_air_done  () 
(cond  ((all  legsjplaced) 
t) 

<t  nil))) 


(dafun  do __place  lags  in  the  air() 

(let  (lag) 

(oond  ((and  (satf  lag  (placable_lag) ) 

(assarta  daoision  (list  'place  lag))) 
t) 

(t  t)  , 

(t  nil)))) 
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•  ********************************************  .■***#******##* 

f 

I  Flan  Libraries  ' 

* 

J  *************  ****************** *********  ****************** 

;  bodyjplan  :-  apeedjplan,  tra jectory_plan. 

;  speed_plan  retract (reduee_speed) ,  slow_down. 

t  spoedjplan  apeed_up. 

;  speed-up  j  -  X  ia  speed_up,j:obot . 

;  alow_down  »-  X  is  alow_down__robot . 

t  tra jectoryjjlan  t-  atablejm,  restore_trajeetory. 

;  tra jectoryjplan  i-  modify-tra jectory? 

l  atablejm  t-  Condition  is  atablejp_m,  Condition  ■■  t. 

;  restore_trajeotory  i-  X  is  restore_aommand. 

;  modify^tra jeetory  s-  X  is  modi£y_oommand. 

t  genarate_deoi*ion  i-  retract (decision (A_leg,B_leg, Ajdeciaion) ) , 
r  X  is  aend_daeiaien7A_leg,BJlag, A_deciaion) ,  Sail. 

i  generate_decisiou  i-  retraot (limit„leg (A_Tegf Ajdeciaion) ) , 

;  x  is  sand_deeiaion (A_leg,”f A^deoision) ,  Sail. 

;  generate_deoieion . 


(defun  body_plan() 

(cond  ( (and  (speed jplan) 

(trajectory  plan) ! 
t) 

(t  nil))) 


(dofun  speed_plan() 

(cond  ((and  (retract  reduce_speed) 
(alow  down) ) 
t) 

((and  (retract  stop  jnovement ) 
(atop_motion)  7 
t) 

((and  (retract  resuir\e_movement) 
(resume_motion)- 
(speed  up) ) 
t) 

(( speed _up)  t> 

<t  nil))) 


(defun  speed_up() 

(cond  (t  (speed  up  robot)  t) 
:t  nil)))- 


(defun  slow  downO 
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(cond  (t  (slow  down  robot)  t) 
<t  nil))) 


(defun  tra j#ctory_plan () 

:  OR 

(oond  ((and  (stablejm) 

(restore  trajectory)) 
t) 

((modify  trajectory)  t) 

<t  nil))) 

(defun  stablejm  (> 

.(oond  ( (stable_p_m)  t) 

(t  nil))) 


(defun  restore_tra jectory () 
(oond  (t  (restore  command)  t) 
(t  nil))) 

(defun  modify_tsa jectory {) 

(oond  (t  (modify_command)  t) 
(t  nil))) 


/  (defun  generate^decision ( ) 
i  (oond  ( (not  (unify  decision  a-deoiaion) ) 

;  nil) 

i  ((and  (unify  decision  a-deoision) 

i  (retract  decision  a-decision) 

I  (print  (list  (aeoond  a-deuision)  (third  deoiaion)  (first  decision))) 

i  (send. decision  (second  a-decision)  (thirdd  decision)  (first  deoiaion) ) 

i  (generate  deoiaion)) 

i  tm 


(defun  ganerate_deoiaion  () 

(oond  ((and  deoiaion 
(not 

(doliat  (a-deoision  decision)  (aend-one-deo.tsion  a-decision))) 
i  dolist  returns  nil 
(not  (aetf  decision  nil)) 
nil)  ;  this  simulates  fail 
t) 

((and  limit_leg 

(unify  limit_lag  daciaionl) 

(oend-one-decinion  decision!) 

(retract  limit  leg  '?) 
nil) 
t) 

(t  t) 

(t  nil))) 


(defun  send-orie-deciaion  (Aeciaion) 
i  format  (decision  legl  leg2) 
i  lisp  function 

(cond  ((equal  (first  decision)  'exchange) 

(send  decision  (aeoond  decision)  (third  decision)  (first  decision) ) ) 
(t 

(send  deoiaion  (second  deoiaion)  nil  (first  decision)))) 


t) 
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i 
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ill  -*-  Mode : Common-Lisp;  BasaslQ 


I**-.********#*****#**************************#***#*******!** 

; 

i  sensor  flavor  definition 

t 

j#**##******#***#*********1****************#****#********** 

(defflavor  ssnsor (state  owner) 

0 

linitable-instance-variables) 


**************  ***  ***********************  ****************** 

/ 

;  contact-sensor  flavor  definition 

/ 

;************»********«****«***************** ************* 

(defflavor  oontaot-oeneor  () 

(sensor) 

: initable-instanoe-variables) 


(deflmeuhod  (aontaot-senaor  tinitti) 
(leg-name) 

(setf  state  (sand  self  tsensing))) 


(defmethod  (sensor  toontaot-p) 
0 

state) 


(defmethod  (sensor  : sensing) 

0 

i  simulation  purpose 
;  graph-terrain  is  object. 

(setf  state 

(let*  ( (leg-pos-wrt-body  (send  (send  owner  lexeoutor) 

:  leg-pos-wrt-body) ) 

(leg-pos-wrt-earth 

(to-earth- transform  (send  owner  iget-Hl)  leg  -pos-wrt-body) ) 
(x-y-pos  (list  (first  leg-pos-wrt-earth) 

(second  leg-pos-wrt-earth) ) ) 

(leg-hoight  (third  leg-pos-wrt-earth))) 

(if  (<  leg-height  (+  0.01  (send  graph-terrain  iget-height  x-y-pos))) 
t 

nil)))) 
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lit  Mode : Common-Lisp;  Base: 10  -*- 

•****®****®******®****»***«******®**************«*******®* 

i 

>  stability-calculator  flavor  definition 

i 

;***®®****®********»**®«******®*®®®*®****«*»«»»***®**®***® 

(daf flavor  stability-calculator (safety-margin 

aafaty-margin-p 

larg«-safety-margin 

larga-safety-margin-p 

reoovary-vactor 

reoovery-veetor-p 

owner) 

0 

: initabla-instance-variables) 


(dafmathcd  (stability-calculator  linitti) 

0 

(sett  safety-margin  0.4) 

(satf  safaty-margin-p  0.2) 

(satf  large-safeuy-margin  0.5) 

(satf  larga-safaty-margin-p  0.4) 

(satf  recovary-vaotor  '(0  0  0)) 

(satf  raoovery-vaotor-p  '(0  0  0))) 


(defmethod  (stability-oalculator  :get-racovary-vector) 

0 

raoovary-vaotor) 


(dafmathod  (stability-oalculator  :get-raoovary-vaotoc-p) 

0 

recove ry-vector-p) 


(dafmathod  (stability-calculator  :convert-to-racovery-vector) 
(stability-vector) 

(let  ( (sm  (first  stability-vector)) 

(vact  (second  stability-veotor) ) ) 

(cond  ( (<  sm  0) 
nil) 

(  (<  sm  0.1) 

(magveot  (/  1  sm)  vact)) 

(t 

(magveot  (/  0.1  (*  am  sm) )  vact))))) 


(dafmathod  (stability-oalculator  :mora-stabla) 

(supporting-lags  H  lagl  lag2) 

(let  ( (stabilityl  (sand  self  icalculate-stability 

1  (cons  lagl  supporting-legs)  H) ) 

(atability2  (sand  calf  icalculate-stability 

(cons  leg2  supporting-legs)  H) ) ) 

(if  (>  stabilityl  stability2) 
t 

nil))) 
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(defmethoct  (stability-calculator  sstable-m) 

/  predict  H  <«  1110 

(supporting-legs  H) 

( let  ( (stability-vector 

(send  self  : get-stability  supporting-legs  K> ) ) 

(oond  ( (>■  (first  stability-vector) 
large -safety-margin) 
t) 

(t 

(if  (>■  (first  stability-vector)  safety-margin) 

(setf  recovery-vector 

(send  self  :eonvert-to-recovery-vector  stability-vector)) 
(setf  recovery-vector  '(0  0  0))) 
nil)))) 


(cefmethcd  (stability-calculator  :stable-p-m) 

;  present*  H  <*  HI 

(supporting-p-legs  K) 

(let*  ( (statiiity-veotor 

(send  self  jget-etability  supporting-p-legs  H) ) 

(st-margin  (first  stability-vector))) 

(oond  ( (>»  st-margin 

large-safety-margin-p) 

t) 

<t 

(setf  recovary-vector-p 

(Sena  self  jconvert-to-reeovery-vector  stability-vector) ) 
(if  <<  at-margin  aafety-margin~p) 

(my-print  (list  'st-p  st-margin))) 
nil)))) 


(defmethod  (stability-calculator  : stable) 
(supporting-logs  H10) 

(It  (>-  (send  self  :calculate-stability 

supporting* legs  H10) 

safety-margin) 

t 

nil)) 


(defmethod  (stability-calculator  :stable-p) 
(aupporting-p-lega  HI) 

(if  (><■  (send  self  :calcuiate-atability 

svpporting-p-legs  HI) 

satiny -margin-  p) 


t 

nil)) 


i 

(defmethod  (stability-calculator  :calculate-stability ) 
(cupporting-legs  H) 

(first  (send  self  :get-ot*bility  supporting-legs  H) ) ) 


^defmethod  (stability-calculator  :get-stability) 


sfaabi lity-t2 . lisp 


Thu  Nov  29  11:35:01  1990 


3 


(supporting-legs  H) 
i if  (>■•*  (counting  aupporting-laga)  3) 

(meaaure-diatence  (center-of-gravity  H) 
(convex-hull 

(aupporting-pointa 
supporting-legs) ) ) 

'  (-100.0  (0  0  0) ) ) ) 


(defun  convex-hull  (pointd) 

;  returns  clockwise-ordered 
/  point  list  of  convex  hull 

(reverse 

(convexl  (oar  points)  points 
'(0  0  0)  nil)),) 


(defun  oor.^axl  (current-point  points  previoua-pt  viaittert-ptS') 

(let*  <  (min-out-pt  (min-out  current-point  previcus-pt  points))  n. 
(pcs  (position  min-out-pt  viaited-pta  itest  #■ equal))) 
(ennd  (poa 

(aubaeq  viaited-pta  0  (+  poa  1))) 

(t  (convexl  min-out-pt 
points 

current -point 

(oona  min-out-pt  visited-pta) ) ) ) ) ) 


(defun  min-out  (ourrent-pt  pv-pt  pta) 

(let*  ( (min-pt  nil) 

(min-angle  100) 

(angle  0)) 

(dolist  <a-pt  pta) 

(cond  ((not  (equal  a-pt  current-pt) ) 

(setf  angle  (turning-angle  (vectsub  current-pt  pv-pt) 

(vectsub  a-pt  current-pt))) 

(oond  ((<  angle  min-angle) 

(aetf  min-pt  e-pt) 

(setf  min-angle  angle)))))) 

min-pt) ) 


(defun  turning-angle (voct  new-vect) 

;  2  D  apace  clock-wise  turning  angle 
;  Neither  vect  should  not  be  zero  vector. 

(let*  ((vectl-0  (list  (first  vect)  (second  vect)  0)) 

(veat2-0  (list  (first  new-vect)  (second  new-vect)  0)) 
(normil-vect  (crossprod  vectl-0  vect2-0)) 

(polarity  (>  (third  normal-vect)  0)) 

(value  (/  (dotprod  vectl-0  vect2-0) 

(*  (magnitude  vectl-0) 

(magnitude  veet2-0)))) 

(angle  0)) 

(if  <>-  value  1) 

(setf  value  1) ) 

(if  (<-  value  -1) 

(setf  value  -1)) 
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(setf  angle  (aeon  value)) 

.  (if  polarity 

<-  (*  2  pi)  angle) 
angle))) 


(defun  center-of-gravity  (K) 

/  oenter-o£-body  is  represented  wrt  earth  coordinate, 
(let  ( (x  (aref  K  o  3) ) 

(y(«ref  H  1  3) > ) 

(list  x  y) ) ) 

/  centet-of-bedy  ban  be  changed  in  future,-. 


(defun.  find-slope  (first-point  second-point) 

(let  ( (d*,l-x  (-  (oar  second-point)  (car  first-point))) 
(del-y  (- 1  (cadr  second-point)  toads  first-point)))) 
(if  (>  (aba  del-x)  0.0000001)  . 

(/del-y  del-x)  . 
nil))) 


(defun  infinite-base  (x  a-line) 

(list  x 

(+  (*  (bar  a-line)  x)  (oadr  a-line)))) 


(defun  interseotion-point  (a-line  b-line) 

i  Returns  list  (x  y) ,  Line  is  list  (slope  oroeaing-point-of-exls) . 

(cond  ( (null  (oar  a-lina) )  (infinite-case  (oadr  a-line)  b-line)) 

((null  (car  b-lina) )  (inf inito-oase  (oadr  b-line)  e-line)) 

(t  (normal-oaee  a-line  b-line)))) 


(defun  in-eide-of-oonvex-hull  (center-point  first-points  second-points) 

(do*  ( (first-points  first-points  (cdr  first-points)) 

(secono-points  second  points  (cdr  second-points)) 

(in-side-f lag  T) ) 

((null  first-points)  in-sida-f lag) 

(if  (test-out-side  (car  first-points)  center-point  (car  second-points)) 
(setf  in-side-flag  nil) ) ) ) 


(dafun  line  (slope  point) 

(if  slope  ' 

(list  slope  (-  (second  point)  (*  sJope  (first  point)))) 

(list  slope  (first  point)))) 

>  When  slope  is  infinitive*  return  with  x-axis  crossing  point  instead  of 
;  y-axis  crossing  point. 


(defun  measure-distance  (center-point  convex-points) 
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;  convex-points  is  a  Hat  of  pointa 
/  point  ia  a  list  (x  y  z)  . 

(let*  ((first-points  convex-points) 

(second-points  (append  (odr  eonvex-pointa) 

(list  (car  first-points))))) 

(if  (in-side-of-convex-hull  center-point  first-points  second-points) 
(start-measure  center-point  firat-pointa  second-points) 

' (-10.0  (0  0  0))))) 

/  center-of-gravity  is  out-side  of  support  pattern 


(defun  normal-case  (a-line  b-line) 
(let*  ((al  (oar  a-line)) 

(bl  (oadr  a-line)) 

<a2  (car  b-line) ) 

(b2  (oadr  b-line) ) 

(x  </  (-  bl  b2)  (-  a2  al)) ) 
(y  (+  (*  al  x)  bl))) 

(list  x  y>>) 


(defun  point-distance  (center-point  first-point  second-point) 

;  returns  distance  and  vector  between  oross-pt  and  center-pt 
(let*  ((slopel  (find-slope  first-point  second  point) ) 

(slope2  (right-angle  slopel)) 

(cross-pt  (intersection-point  (line  slopel  first-point) 

(line  alope2  center-point))) 
(del-vect  (veotsub  center-point  cross-pt  ) ) 

(distance  (magnitude  del-veet) ) ) 

(llat  distance  (list  (firat  del-vect)  (second  dsl-veot)  0.0)))) 


(defun  right-angle  (slope) 

(oond  ((null  slope)  0.0)  /  infinitive  input  slope 

( (<  (abs  slope)  0.0000001)  nil)  i  zerop  slope 
(t  (/  (-  1)  slope))) > 


(defun  start-measure  (canter-point  first-points  second-points) 

(do*  ( (firat-pointa  first-points  (cdr  first-points) ) 

(aeoond-points  second-points  (cdr  second-points) ) 

(min-distanos  10000.0  min-ilistanos)  ;  infint#  dummy  number  10000.0 
(min-direotion  nil)  (dis-dir  nil)) 

((null  first-points)  (list  min-distanos  min-dirsction) ) 

(astf  dia-riir  (point-distance  center-point 

(oar  f irat-points)  (car  aecond-pointo ) ) ) 
(cond  ( (<  (first  dis-dir)  min-distanos) 

(setf  min-distance  (firat  dis-dir) ) 

(setf  min-diraction  (second  dis-dir)))))) 


(defun  supporting-points  (loga) 
(mapcar  #' (lambda  (leg) 

(send  leg  : foothold)) 

legs) ) 
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(dafun  tast-out-aida  (first-point  sacond-point  third-point) 

(lat*  ((a  (-  (cadr  first-point)  (cadr  third-point)!) 

(b  (-  (car  third-poi..:)  (car  first-point))) 

(c  (-  (+  (*  a  (car  third-point))  (*  b  (cadr  third-point))))) 
(dacision  (+  ( *  a  (car  sacond-point!) 

<*  b  (cadr  sacond-point)) 
c))) 

(if  (>-  dacision  0.0) 

T 

nil))) 


i 
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n:  -*-  Mod*: Common-Lisp;  Base: 10  -*- 

/ 

/  stop-body  definition 
; 

j*****************************»*w**************************** ******* 


(defflavor  stop-body  (stop-body-motion-flag) 
(body) 

linitable-instance-variables) 


(dsfmsthod  (stop-body  t after  linitti) 

0 

(setf  stop-body-motion-flag  nil)) 


(defmethod  (stop-body  i stop-body-motion) 

0 

(setf  stop-body-motion-flag  t)) 


(defmethod  (stop-body  lreatore-body-motion) 
0 

(setf  atop-body-motion-flag  nil) ) 


(defmethod  (stop-body  loaloulate-motion) 

( joyatiok-command  legs) 

(setf  joy-command  joystiok-command) 

(oond  ( (equal  support-plane-olock  10) 

(setf  estimated-support -plane 

(send  support -plane -estimator  iget-plane  legs) ) 

(setf  support-plane-olock  0))) 

(setf  support-plane-olock  (+  support-plane-olock  1)) 

(cond 

((or  atop-motion-flag  stop-body-motion-flag  (null  modify-veotor-p) ) 
(send  body-controller  icontrol 
'(00  0) 

0  estimated-support-plano) ) 

(modify-veotor-p 
(send  body-controller  icontrol 

(veotadd  joy-command  (send  self  iget-modify-veotor-p) ) 
deceleration-factor  estimated-support -plane) ) 

(t 

(oontrol  body-controller 

(veotadd  joy-command  (sand  self  ;get-modify-veotor) ) 
deceleration-factor  estimated-support-plane) ) ) ) 
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ill  -*-  Mode: Common-Lisp;  Base: 10  -*- 

j*********ilr*****ilr*********#****#****itriir**i»f*****»iirt**il»******rt 

/ 

;  support-plane-eatimator  flavor  definition 
; 

j********************************************************* 


(def flavor  support -plane-estimator (owner) 

0 

) 


(defmethod  (support -plane-estimator  tinitti) 

0 


) 


(defmethod  (aupport-plane-estimator  iget-plane) 

(lege) 

(let*  ( (footholde-for-eetimation  (get-footholda  legs)) 

(oonstante  (get-oonatants  footholda-for-eatimation) ) ) 
(make-plane-f rom-ooeff ioient  constants) ) ) 


;*************#**********#******;*****#*#****************** 

; 

I  support-plane-eotimator. get-plane 
i 


(defun  add-pointa  (points) 

i  returns  a  list  (number-of-points  sum-of-points) . 
(do  ((points  points  (odr  points)) 

(i  0  (+  i  1)) 

(sum-vect  '  (0  0  0) ) ) 

((null  points)  (list  i  sum-vest)) 

(setf  aum-veot  (vectadd  (car  points)  sum-vect)))) 


(defun  average-point  (points) 

(let*  ( (num-i-sum-vect  (add-pointa  points)) 

(number -of -point a  (first  num-4-sum-vect) ) 

(sum-veot  (second  num-4-sum-vect) ) ) 

(if  (>  number-of-points  0) 

(magvect  </  1  number-of-points)  sum-veot) 

(print  "Error  in  finding  average-point  of  estimate  plane")))) 


(defun  get-aO  (bar-point  al) 

(let*  ( (x-bar  (first  bar-point)) 
(s-bar  (third  bar-point))) 
(-  z-bar  (*  al  x-bar) ) ) ) 
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(dafun  gat-al  (points  bar-point  common-denominator) 
l  ratusns  al  which  is  sun  in  this  function. 

(do*  ((points  points  (odr  points)) 

(sum  0) 

(x  nil)  (x-bar  (first  bar-point)) 

(anil)  (a-bar  (third  bar-point) ) ) 

((null  point*)  (/  sum  common-denominator)) 
(satf  x  (first  (oar  points))) 

(satf  a  (third  (car  points))) 

(satf  sum  <+  sum  (*  (-  x  x-bar)  (-  a  a-bar)))))) 


(dafun  gat-a2  (points  bar-point  eommon-danominator) 

;  raturns  a2  which  is  sum  in  this  function. 

(do*  ((points  points  (cdr  points)) 

(sum  0) 

(x  nil)  (x-bar  (first  bar-point)) 

(y  nil)  (y-bar  (second  bar-point) ) ) 

((null  points)  U  sum  eommon-danominator) ) 
(satf  x  (first  (oar  points))) 

(satf  y  (second  (oar  points))) 

(satf  sum  (4-  sum  (*  (-  x  x-bar)  (-  y  y-bar)))))) 


(dafun  g*t-a3  (bar-point  a2) 

(let*  ((x-bar  (first  bar-point)) 
(y-bar  (second  bar-point) ) ) 
(-  y-bar  <*  *2  x-bar) ) ) ) 


(defun  get-a4  (points  aO  al  a2  a3) 

(let*  ( (number-of-points  (counting  points) ) 

(yr  (get-yr  points  a2  *3)) 

(sr  (get-sr  points  aO  al)) 

(yr-bar  (gat-yr-bar  yr  number-of-points) ) 

(zr-bar  (gat-sr-bar  sr  number-of-points) ) ) 

(do  ( (yr  yr  (cdr  yr) ) 

(zr  zr  (cdr  zr) ) 

(numerator  0)  (a-yr  0)  <a-zr  0) 

(denominator  0)) 

((null  yr)  (/  numerator  denominator)) 

(setf  s-yr  (first  yr)) 

(setf  s-zr  (first  sr)) 

(setf  numerator  (+  numerator  (*  (-  a-yr  yr-bar)  (-  a-zr  zr-bar)))) 
(setf  denominator  (+  denominator  (*  (-  a-yr  yr-bar)  (-  a-yr  yr-bar))))))) 


(dafun  get-oommon-danominator  (points  bar-point) 

(do*  ((points  points  (cdr  points)) 

(sum  0)  1 

(x  nil) 

(x-bar  (first  bar-point))) 

( (null  points)  sum) 

(satf  x  (first  (car  points))) 

(satf  sum  (+  sum  (*  (-  x  x-bar)  (-  x  x-bar)))))) 
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(dafun  gat-constants  (points) 

(1st*  ((bar-point  (avaraga-point  points)) 

(common.-danominator  (gat-conwon-danominator  points  bsr-point)) 
(al  (gst-al  points  bar-point  common-danominator) ) 

(a2  (gst-a2  points  bar-point  eommon-dsnominator) ) 

(aO  (gst-aO  bar-point  al)) 

(a3  (gat-a3  bar-point  a2)) 

(a4  <gat-a4  points  aO  al  a2  a3) ) ) 

(list  aO  al  a2  a3  a 4 ) ) ) 


(dsfun  gat-footholds  (lags) 

(do*  ((lags  lags  (ode  legs)) 

(footholds  nil) 

(a-lsg  nil) ) 

((null  lags)  footholds) 

(satf  a-lag  (ear  lags) ) 

(if  (sand  a-lag  i foothold) 

(satf  footholds  (oona  (send  a-lag  i foothold)  footholds))))) 


(dafun  gat-yr  (points  a2  a3) 

(do*  ((points  points  (odr  points)) 

(yr  nil) 

(x  nil) 

(y  nil)) 

((null  points)  (ravarsa  yr) ) 

(satf  x  (first  (oar  points) ) ) 

(satf  y  (saoond  (oar  points))) 

(satf  yr  (cons  <-  y  «2  <*  a3  x) )  yr) ) ) > 


(dafun  gat-ys-bar  (yr  numbar-of-points) 

(do  ( (yr  yr  (odr  yr) ) 

(yr-bar  0) ) 

((null  yr)  (/  yr-bar  numbar-of -points) ) 
(satf  yr-bar  (+  yr-bar  (first  yr) ) ) ) ) 


(dafun  gat-zr  (points  aO  al) 

(do*  ((points  pointc  (odr  points)) 

(zr  nil) 

(x  nil) 

(z  nil)) 

((null  points)  (ravaraa  zr) ) 

(satf  x  (first  (oar  points))) 

(satf  z  (third  (oar  points))) 

(satf  zr  (oons  (-  z  aO  (*  al  x) )  zr)))) 


i 

(dafun  gat-zr-bir  (zr  nuwbar-of-points) 

(do  ( (zr  zr  (odr  zr) ) 

(zr-bar  0) ) 

((null  zr)  (/  zr-bar  numbar-of-points) ) 
(satf  zr-bar  (+  zr-bar  (first  zr))))) 
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(defiun  maka-plane~f rom-coefficient  (constants) 

(1st*  ( (aO  (first  constants)) 

(al  (second  constants)) 

<a2  (third  constants) ) 

(a3  (fourth  constants)) 

(a4  (fifth  constants)) 

(a  <-  <*  a4  a 3 )  al) ) 

(b  (-  a4)  ) 

(c  1) 

<d  (-  <»  a2  a 4 }  aO) ) 

(unit-normal  (normalire-vector  (list  a  b  a))) 
(dia  (/  d  (magnitude  (list  a  b  c))))) 

(list  unit-normal  dia))) 


terrain-ragulator-t.lisp  Thu  Nov  29  11:35:38  1990  1 

III  ModOiCommon-Liap;  Saaa:10 

;#**********#***«*******•***************#*************»*** 

• 

;  tarrain-ragulator  flavor  definition 
/ 

;«1t*****»************************W»*W***ll****»*********** 


(daf flavor  tarrain-ragulator (body-rotata-rata-x  body-rotata-rata-y 

body-trana-rata-s  old-body-rutata-rata-x 
old-body-rotata-rata-y  old-body-trana-rate-s 
gain  min-haight  max-haight 
atal  ata2  min-ata  max-ata) 

(regulator' 

sinitable-inatanoe-variabiea) 


(dafmathod  (tarrain-ragulator  linitfei) 

0 

(aatf  gain  5) 

i  0  dagxaa 
/  29  dagraaa 
i  4.4  faat 
/  5.4  faat 
f  0  dagraa 
t  30  dagraa 


(aatf  min-ata  0.0000001) 
(aatf  max-ata  0.4343) 
(aatf  min-haight  4.4) 
(aatf  max-haight  3.4) 
(aatf  atal  min-ata) 

(aatf  ata2  0.9236) 


(aatf  body-rotata-rata-x  0.0) 

(aatf  body-rotata-rata-y  0.0) 

(aatf  body-trana-rata-a  0.0) 

(liat  body-rotata-rata-x  body-rotata-rata-y  body-trana-rata-a) ) 


(dafmathod  (tarrain-ragulator  ido-tarrain-rugulation) 
(k-gamma-dalta-haight ) 

t  k-gamma-dalta-haight  is  ((k.xk.yk.a)  gamma  dalta-haight) . 

(lat*  ( (k  (first  k-gamma-dalta-haight)) 

(gamma  (aaoond  k-gamma-dalta-haight)) 

(dalta-haight  (third  k-gamma-dalta-haight) ) 
(body-rotata-rata-x-n  (*  gain  (first  k)  gamma)) 
(body-rotata-rata-y-n  (*  gain  (aaoond  k)  gamma)) 
(body-trana-rata-s-n  (»  gain  dalta-haight))) 

(aotf  body-rotata-rata-x 
(aand  aalf  tlimitor 

(sand  salf  sfiltar  body-rotate-rata-x-n  body-rotata-rata-x) 

0.1)) 

(aatf  body-rotata-rata-y 
(aand  aalf  :limitor 

(sand  salf  Jfiltar  body-rotata-rata-y-n  body-rotata-rata-y) 

0.1) ) 

(aatf  body-trana-rata-s 

(aand  aalf  ilimitor 

(aand  waif  :filtar  body-trans-rata-s-n  body-trana-rata-s) 
1))) 

(liat  body-rotata-rata-x  body-rotata-rata-y  body-trana-rate-r) I 


(dafmathod  (tarrain-ragulator  :ata-function) 
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(eta) 

(let  ((slope  (/  (-  max-eta  min-eta)  <■  eta2  etal)))) 
(+  min-eta  (•  slope  <-  eta  etal))))) 


(defmethod  (terrain-regulator  iget-k-gamma-by-alope) 

(plant  H) 

(let*  ( (plane-rpt-body  (plane-transform  plane  H) ) 

(height  (cadr  plane-rpt-body) ) 

(eta  (aro-oos  (third  (ear  plane) )) ) 
(k-gamma-desired-haight  nil) ) 

(setf  k-gamma-desired-height 

(oond  ((<  eta  etal)  (send  self  ilow-slope  plane)) 

( (<  eta  eta2)  (send  self  imid-slope  eta  plane  H) > 
<T  (send  self  thigh-slope  plane  H) ) ) ) 

(list  (first  k-gamma-desired-height) 

(seoond  k-gamma-desired-height) 

(-  (third  k-gamma-desired-height)  height) ) ) ) 


(defmethod  (terrain-regulator  iheight-funetion) 

(eta) 

(let  ((slope  (/  (-  max-height  min-height)  (-  eta2  etal)))) 
(-  max-height  (*  slope  (-  eta  etal))))) 


(defmethod  (terrain-regulator  thigh-slope) 

(plana  H) 

(let*  ( (plane-unit -normal  (first  plane) ) 

(a  (first  plane-unit-normal)) 

(b  (seoond  plane-unit-normal) ) 

<m  (eqrt  <+  <*  a  a)  (*  b  b) ) ) ) 

(desired-eta  max-eta) 

(desired-height  min-height) 

(desired-body-plane  (list  (list  (*  (/  a  m)  (sin  desired-eta)) 

(*  (/  b  m)  (ain  desired-eta)) 

(oos  desired-eta))  0.0)) 

(deaired-body-plane-in-body  (plane-transform  desirad-body-plane  H) ) 
(unit-normal-body-plane  (first  deeired-body-plane-in-body) ) 

(Hi  (first  unlt-normal-body-plsne) ) 

(bl  (second  unit-normal-body-plane) ) 

(ol  (third  unit-normal-body-plane!) 

(ml  (aqrt  (+  (*  el  al)  (*  bl  bl)))) 

(k  (if  (-  ml  0) 

(list  000) 

(list  (/  <-  bl)  ml)  (/  si  ml)  0)) ) 

(gamma  (aro-ooa  ol))) 

(list  k  gamma  deaired-height) ) ) 


(defmethod  (.terrain-regulator  ilimitor) 
(vel  max-val) 

(if  (>-  (aba  vel)  max-val) 

(if  (>  vel  0) 
max- vel 
(-  max-val) ) 
vel)) 
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(dafmathod  (tarrain-ragulator  ilow-alope) 

(plan*) 

(lat*  ((unit-normal  (first  plan®)) 

(a  (first  unit-normal)) 

(b  (aacond  unit-normal) ) 

<c  (third  unit-normal)) 

<m  (sqrt  (+  <*  a  a)  (*  b  b) )  ) ) 

(k.a  nil) 

(k.b  nil) 

(gamma  (aro-oos  c)) 

(daairad-haight  max-haight)) 

(if  (•  m  0.0) 

(satf  k.a  0.0  k.b  0.0) 

(satf  k.a  (/  (-  b)  m)  k.b  (/  a  m) ) ) 

(list  (list  k.a  k.b  0.0)  gamma  daairad-haight) ) ) 


(dafmathod  (tarrain-ragulato;  tmid-alops) 

(ate  plana  H) 

(lat*  ( (plana-unit-normal  (first  plana)) 

(a  (first  plana-unit-normal)) 

(b  (aacond  plane-unit-nomal) ) 

(m  (sqrt  <+  (*  a  a)  (*  b  b) )  > ) 

(daairad-ata  (sand  salf  lata-funotion  ata)  > 

(daairad-haight  (sand  salf  ihaight-function  ata) ) 
(dasirad-body-plana  (list  (list  !*  \/  a  m)  (sin  deairad-ata) ) 

(*  (/  b  m)  (sin  dasirad-ata) ) 

(ooa  dasirad-ata))  0.0)) 

(daairad-body-plana-in-body  (plana-transform  daairad-body-plana  H) ) 
(unit-normal-body-plan*  (first  dasirad-body-plana-in-body)) 

(al  (first  unit-normal-body-plana) > 

(bl  (saoond  unit-normal-body-plana)) 

(ol  (third  unit-normal-body-plana) ) 

(ml  (sqrt  (+  (*  al  al)  (*  bl  bl)))) 

(k  (if  (-  ml  0) 

(list  0  0  0) 

(list  (/  (-  bl)  ml)  </  al  ml)  0))) 

(gamma  (aro-oos  cl))) 

(list  k  gamma  dasirad-haight) )  ) 


(dafmathod  (tarrain-ragulator  iragulata) 

(astimatad-support-plana  H) 

(lat  ((k-gamma  (aand  aalf  igat-k-gamma-by-alopa  astimatad-support-plana  H))) 
(sand  aalf  ido-tarrain-ragulation  k-gamma))) 


(dafmathod  (tarrain-ragulator  irastora) 

0 

(aatf  body-rotato-rata-x  old-body-rotata-rata-x) 

(satf  body-rotata-rata-y  'old-body-rotata-rata-y) 

(satf  body-trana-rata-r  old-body-trans-rate-s) 

(list  body-rotata-rata-x  body-rotsta-rata-y  body-trans-rata-s) ) 


(dafmathod  (tarrain-ragulator  :save) 

0 

(satf  old-body-rotata-rata-x  body-rotate-rate-x! 
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(satf  eld-body-rotat#-rata-y  body-rotato-rate-y) 
(setfi  old-body-trana-rate-z  body-trans-rate-z) 

) 


i 
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in  Mode  sCommon-Lisp;  Ease:  10  -*- 

I 

;  test-overlap-leg  definition 
> 

.***<>***«***************<  ******  t****»**w**'-***i«»*»*******1ir**, **„»**» 


(dof flavor  test-overlap-leg  0 
(overlap-.leg) 

) 


(defmethod  (test-overlap-leg  ichenge-to-back-foothold) 
0 

(setf  foothold  (first  foothold-list)) 

(setf  tkm  (first  tkm-list) ) ) 
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...  Mode: Common -Map;  Base:  10 

j*»#ilf************rt  ************************************************** 

! 

i  teat-overlap-robot  definition 
; 


(def flavor  teat-overlap-robot  <) 
(overlap-robot) 

) 


(defmethod  (teat-overlap-robot  sinitti) 

0 

(send  greph-asv  :init-data) 

(aetf  vision-system  (make-instanco  'ditch-viaion-system  sowner  self)) 
(send  vision-system  sinitti) 

(aetf  joystick  (make-instance  ' joystick}) 

(send  joystick  i reset) 

(empty-etueue  lift-queue) 

(aetf  lift-flag  t) 

(let  ((H)) 

(aetf  body  (make-inatanaa  'body  sowner  self)) 

(aetf  H  (send  body  sinitti)) 

(setf  legs  (list 

(make-instance  'test-overlap- leg  :name  'legl  sowner  aelf) 
(make-inntance  'teat-overlap-leg  tname  'leg2  towner  aelf) 
(make- instance  ' test-overlap- leg  tname  'leg3  towner  seif) 
(make-instance  ' test -overlap-leg  tname  'leg4  sowner  self) 
(make-instance  'test-overlap-leg  tname  'leg5  towner  self) 
(make-inatanoe  'test-overlap-leg  sname  'leg6  towner  self) 
>> 

(mapcar  #' (lambda  (a-leg)  (send  a-leg  sinitti  H) )  legs)) 


(defmethod  (test-overlap-robot  : send-decision) 
(legl  leg2  a-decision) 

(cond  ((equal  a-decision  'exchange) 

(send  legl  : send-decision  a-decision) 
(send  leg2  : send-decision  'piace) 

(send  legl  : send-axchange  leg2)) 

( (equal  a-decision  'place_back) 

(send  legl  s change-to-back-f oothold) 
(send  legl  s send-decision  'place)) 

(t 

(send  legl  : send-decision  a-decision)))) 


(defmethod  (test-overlap-robot  shas-more-tkm) 
(legl  leg2) 

(let  ( (tkml  (send  legl  stkm)) 

(tkm2  (send  leg2  stkm))) 

(cond  ((null  tkm2) 
t) 

((null  tkml) 
nil) 

( (>  tkml 

tkm2) ) ) ) ) 
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(aefmethod  (test-overlap-robot  :  lif table-leg) 

(leg) 

(oond  ( (mamba r  lag  lif t-able-legs  stcat  I'equal) 
lag) 

<t  nil))) 


(defmethod  (tast-overlap-robot  iplSoable-leg) 

(leg) 

(oond  ((and  (member  leg  place-able-legs  :taat  #'aqual) 
(send  lag  thas-foothold-p) ) 

leg). 

(t  nil))) 


(defmethod  (test-overlap-robot  i lifteble-front-leg) 

0 

(oond  ((send  self  t liftable-lag  (first  lags))) 

( (send  self  ilif tab) e-leg  (second  lags) ) ) 

<t  nil))) 


(defmethod  (tast-overlap-robot  iplaoable-front-leg) 
0 

(oond  ((send  aelf  :placable-leg  (first  lags))) 
((send  sell;  iplaaable-lag  (saoond  legs))) 

<t  nil))) 


(defmethod  (tast-overlap-robot  :both-f ront-legs-piaead) 

0 

;  If  one  of  front  legs  has  not  foothold,  than  this  will  fail! 

(cond  ((and  (member  (firat  lags)  support ing-p-legs  :tast  f’aqual) 

(member  (saoond  lags)  supporting-p-legs  :tast  f'equal)) 
t) 

(t  nil))) 


(defmethod  (test-overlap -robot  :both-front-lags-lif ted) 

0 

(oond  ((and  (not  (member  (firat  lags)  aupporting-p-leg«  steat  #' equal)) 

(not  (member  (saoond  legs)  supporting-p-legs  :test  #'equal))) 
t> 

(t  nil))) 


(defmethod  (test-overlap-robot  : liftable-rear-leg) 

0 

(cond  ((sand  self  :liftable-leg  (fifth  legs))) 
((sand  self  : liftabla-leg  (sixth  legs))) 

(t  nil)) ) 
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(dafmathod  ( cast-ovnrlap-robot  :placabla-rear-lag) 
0 

(oond  ({sand  salf  :placabla-lag  (fifth  legs))) 
((sand  salt  iplacnble-leg  (sixth  lags))) 

(t  nil))) 


(dafmathod  (tact-ovnrlap-robot  sboth-raar-laga-plactd) 

0 

1  it  ona  of  raar  lags  has  not  foothold,  than  this  will  fail) 

(oond  ((and  (manbar  (fifth  lags)  aupporting-p-iags  :tast  I'aqual) 
(numbar  (sixth  lags)  aupport*ng-p-laga  staat  I'aqual)) 
f) 

(t  nil))) 


(dafmathod  (tast -ovarlap-robot  sboth-raar-lsgs-liftad) 

0 

(oond  ((and  (not  (mambar  (fifth  lags)  support ing-p-lags  ttaat  I'aqual)) 
(not  (mairbar  (sixth  logs)  aupporting-p-lags  :tast  I'aqual))) 
t; 

(t  nil))) 


(dafmathod' (taat-ovarlap-robot  : liftsbla-middla-lag) 
0 

(cond  ((sand  salf  iliftabla-lag  (third  lags))) 
((sand  salf  i liftabla-lag  (fourth  lags))) 

<t  nil))) 


(dafmathod  (taat-ovarlap-rohot  tplaoablc-middla-lag) 
0 

(cond  ((sand  salf  :placsb)a-lag  (third  lags))) 
((sand  aali  :placabla-lay  (fourth  lags))) 

(t  nil))) 


(dafmathod  (tast-ovarlap-robot  sboth-middla-lags-plsoad) 

0 

/  If  ona  at  middla  lags  has  not  foothold,  than  this  will  fail! 

(oond  ((and  (mambar  (third  lags)  aupporting-p-laga  start  I'aqual) 

imambar  (f,  :ruh  lags)  supporting-p-lags  ;tast  I'aqual)) 
U 

(t  nil) ) ) 


(dafmathod  (tast-ovaelsp-robot  iboth-middla-laga-liftad) 

0 

(cond  ((and  (not  (mambar*  (third  lags)  aupporting-p-lags  :tast  I'aqual)) 

(not  (mambar  (fourth  lags)  supporting-p-lags  :t*3t  I'aqual))) 
t) 

<t  nil) )) 


(dafmathod  (test-overlap-robot  : onw-piacsbla-iag) 
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(cond  ({sand  self  :placable-f ront-ley)  ) 

((•end  self  iplacable-middle-leg) ) 

(  (send  self  :plaeabla-reir-leg) ! 
it  nil))) 


(defmethod  (teat-overlap-robot  lall-legs-plaeed) 

0 

(cond  {(and  (send  self  iboth-front-iegs-plaeed) 
(send  self  ibeth-middle-lags-placed) 
{send  self  iboth-rear-legs-placed) ) 


t) 

(t  nil))) 


(defmethod  (ttest-overlap-robot  tat-ditch-area) 
0 

(send  vision-system  !on-ditch-ar«a 
(send  self  :get-HlO))) 


j************************************************************ 

/ 

I 

i  Prolog  Interface  Functions 
/ 

; 

;***********#*************  ******************* *************  A** 

(defun  liftable_front_lag  0 
(send  asv  :liftable~front-leg) ) 


(defun  placabla_front_leg  0 
(send  asv  splacable~£ront-lag) ) 


(defun  bo th_front_legs. .placed  () 

(send  asv  :both-£ron’t-lega-placed) ) 


(defun  both_f  rer.t_legs_lifted  <) 

(send  asv  jboth-f ront-lega-lifted) ) 


(defun  liftable_rear_leg  () 

(send  asv  :  liftable-reanleg) ) 


(defun  placable_rear_l«g  {) 

(send  asv  : placable-rear-leg) ) 


(defun  both_rear..legs_placed  (> 

(send  asv  :both-rear-lega-placed) ) 


l 
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(dafun  both_raar_laga_li£t#d  () 

(sand  aav  :both-eaar-laga-lifted) ) 


(dafun  liftablajniddlaJLag  <) 

(sand  aav  sliftabla-middla-lag) ) 


(dafun  plaoabla_middla  lag  () 

(sand  aav  :placabla-iniddla-lag) ) 


(dafun  both_middla—lage_jlaoad  () 

(sand  aav  :both-middla-lags-placad) ) 


(dafun  bothjniddla_lags  llftad  () 

(sand  aav  iboth-middla-lags-liftad) ) 


(dafun  placabla_lag  () 

(sand  aav  :ona-placabla-lag) ) 


(dafun  all_lags_plaoad  () 

(sand  aav  :all-laga-piacad) ) 


(dafun  at__ditoh_araa  () 

(sand  aav  :at-ditch-*raa) ) 


I 
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til  -*-  Mod* :Common-Lisp;  Base: 10  -*- 

J a************************************ ****************** 
; 

;  tkm-calculatcr  flavor  definition 
l 

j*******#**rt*********#****w**w**#*#itr************iirwiifW**w» 


(def flavor  tkm-calculator {working-volume  owner) 

0 

sinitable-instance-variables) 


(defmethod  (tkm-calculator  linitti) 
(leg-name) 

(oond  ((equal  leg-name  'legl) 
(setf  working-volume 

'<<((0  0  1)  3.316) 
((<0  0  1)  5.7313) 
((equal  leg-name  'leg2) 
(setf  working-volume 

' <  ( ( (0  0  1)  3.316) 
(((0  0  1)  5.7313) 
((equal  leg-name  'leg3) 
(setf  working-volume 

'  (((<0  0  1)  3.316) 
(((0  0  1)  5.7313) 
((equal  leg-name  ' leg4) 
(setf  working-volume 

'  ((((0  0  1)  3.316) 
(((0  0  1)  5.7313) 
((equal  leg-name  'legS) 
(setf  working-volume 

'((((0  0  1)  3.316) 
<<(0  0  1)  5.7313) 
((equal  leg-name  'leg6) 
(aetf  working-volume 

'((<(0  0  1)  3.316) 
(((0  0  1)  5.7313) 

) 


(<1 

0 

0) 

-8. 0832) 

((0 

0.9397 

0.3420) 

((1 

0 

0) 

-3.4167) 

((0 

0.9397  -0.3420) 

<<1 

0 

0) 

-8.0632) 

((0 

0.9397 

0.3420) 

((1 

0 

0) 

-3.4167) 

((0 

0,9397  -0.3420) 

<<1 

0 

0) 

-2.2915) 

((0 

0.9397 

0.3420) 

((1 

0 

0) 

2.2915) 

((0 

0.9397  -0.3420) 

<<1 

0 

0) 

-2.2915) 

((0 

0.9397 

0.3420) 

((1 

0 

0) 

2.2915) 

((0 

0.9397  -0.3420) 

((1 

0 

0) 

3.3332) 

((0 

0.9397 

0.3420) 

((1 

0 

0) 

7.8332) 

((0 

0.9397  -0.3420) 

( (1 

0 

0) 

3.3332) 

((0 

0.9397 

0.3420) 

( (1 

0 

0) 

7.8332) 

((0 

0.9397  -0.3420) 

(defmethod  (tkm-caloulator  ;find-tkm) 

(a-foothold  body-trans-rate  body-rotate-rate) 

/  a-foothold  ia  based  on  body  coordinate 
;  returns  tkm 

(let*  ( (leg-vel-rpt-body 

(get-leg-velocity 

a-foothold  body-trana-rate  body-rotate-rate))) 
(get-tkm  a-foothold  leg-vel-rpt-body  working-volume))) 


(defun  get-distance  (planes  velocity  leg-position) 

;  global  function  :  plane-distance 
;  beforo  start,  make  one  plane  list 

(do  ((planes  (append  (first  planes)  (second  planes))  (cdr  planes)) 
(a-tkm  nil) 

(min-tkm  10000) ) 

( (null  planes)  min-tkm) 

(setf  a-tkm  (plane-distance  (car  planes)  velocity  leg-position) ) 


2.569) ) 

2.569) )))) 


2.569) ) 

2.569) )))) 


2.569)  ) 

2.569) )))) 


2.569) ) 

2.569) )))) 


-2.569)) 

-2.569))))) 


2.569) ) 
2.569))  ))) 
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(if  a-tkm 

(if  (and  (>  a-tkm  0)  (>  min-tkm  a-tkm)) 
(setf  min-tkm  a-tkm))))) 


(defun  get-lag-velocity  (poa-rpt-body  body-trana-rate  body-rotate-rate) 
i  returns  leg-velooity-wrt-body 

/  ■  -  (  body-trans-rete  +  body-rotate-rate  x  pos-rpt-body  ) 

(vecteub  MO  0  0) 

(vectadd  body-trana-rate 

(erossprod  body-rotate-rate  poa-rpt-body) ) ) ) 


(defun  get-tkm  (leg-poa-rpt-body  velocity  working-volume) 

I  global  function  i  magnitude 

;  outside  w.v  returns  nil.  If  speed  is  near  0,  then  returns  1000.0. 

(if  (in-side-volume  leg-poe-rpt-body  working-volume) 

(let  ((speed  (magnitude  velooity) ) ) 

(if  (<  speed  1/1000) 

1000.0 

(/  (get-distanoe  working- volume  velooity  leg-pos-rpt-body)  speed))) 

nil)) 


(defun  in-side -volume  (position  planes) 
i  planes  ((up  front  left)  (beak  right  bottom)) 

(let*  ((positive-planes  (first  planes)) 

(negative-planes  (second  planes) ) 

(inside-flag  T) ) 

(dolist  (a-plane  positive-planes) 

(if  (>-  (plane-normal-distanoe  a-plane  position)  0) 
(setf  inside-flag  nil))) 

(dolist  (a-plane  negative-planes) 

(if  (<«  (plane-normal-distance  a-plane  position)  0) 
(setf  inside-flag  nil))) 
inside-flag) ) 


t 
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til  -*-  Mode :Common-Liap;  Base: 10  -*- 

; 

;  user  interface  routines 

; 

j*************** **************************************************** 


(defvar  *old-terrain-file-name*) 
(defvar  *new-terrain-fi ile-name*) 
(defvar  *tarrain-alope-type*) 
(defvar  *terrain-alopa-data*) 
(defvar  *tarrain-slopa-angle* ) 
(defvar  *ter rain-type*) 

(defvar  *obstacle~ratio*) 

(defvar  * random-seed*) 

(defvar  *ok-£lag*) 

(defvar  *soreen*) 

(defvar  *screen-width*) 

(defvar  *acraen-height*) 

(defvar  *new-lisp-listener*) 
(defvar  *ditch-width*) 

(defvar  *ditch-location*) 

(defvar  *ditoh-type*) 


(defun  initialize-menu-variables  () 
(setf  *old-terrain-f ile-name*  nil) 
(setf  *new-terrain-f ile-name*  nil) 
(setf  *terxain-alope-type*  'default) 
(setf  *terrain-alope-dafca*  nil) 

(setf  *terrain-slope-angle*  0) 

(setf  *terrain-type*  'random) 

(setf  *obstaole-ratio*  '25) 

(setf  *random-seed*  '123) 

(setf  *ok-flag*  t) 

(setf  *ditoh-width*  6) 

(setf  *ditoh-location*  21) 

(setf  *ditoh-type*  'no-ditch)) 


(dafun  get-old-terrain-f ile-name ( ) 

(let  ((file-names 

(mapcar  #' (lambda  (file) 

(list  (file-namestring  file)  documentation  "Uie  an  old  terrain*) 
(directory  "robot : kwalc . robot .terrain-data; * .*")))) 

(if  file-names 

(wsmenu-choose  (cons  ' ("new-terrain"  lvalue  nil  : documentation  "Create  a  new  ter 

in") 

file-names) 

: label  "Select  terrain" 
ssuperior  *new-lisp-listener*) 

(w:menu-choose  '(("new-terrain"  tvalue  nil  : documentation  "Create  a  new  terrain" 
i label  "Select  terrain" 

:superior  *new-liap-listener*) ) ) ) 


(defun  get-terrain-slope-type () 

(w : choose-variable-values 
'  ( (*tarrain-slope''type*  :menu-alist 


( ("Default"  :value  default) 

("Single  Angle"  :value  single-angle) 
("Manual"  : value  manual)))) 
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: label  "Choose  terrain  slope  profile" 
ssuperior  *new-lisp-listener*) 

* ter rt in-slope-type*) 


(defun  get-terrain-slope-angle  <) 

(wichoose-variable-values 

' ( ("tersain-slope-angle*  t document at ion  "Input  terrain  slope  angle" 

iconstraint  (lambda  (dl  d2  d3  value) 

(oond  ((>  (abs  value)  30)  "Too  steep  angle") 
(t  nil))))) 

i label  "Input  terrain  slope  angle" 

(superior  »new-lisp-listener*) 

*terrain-slope-angle*) 


(defun  get-terxain-alope-data () 

(setf  *terrain-alope-data*  '((IS  0)  <30  2))) 

(vuchoose-variable-valuea 

' ( (*terrain-elope-data*  (documentation  "Input  format  ( (xl  hi)  (x2  h2)  ...  )" 

(constraint  (lambda  (dl  d2  d3  value) 

(cond  ((null  value)  "Please  input  slope") 
(t  nil))))) 

(label  "Input  slope  data" 
isuperior  *naw-lisp-listener*) 

*terrain-alope-data*) 


(defun  get-terrain-obstaole-type  () 

< w i ohoose-var iable-valuaa 

' ( (‘terrain-type*  imenu-alist  (("Random"  (value  random) 

("Manual"  (value  manual) ) ) ) 

(label  "Choose  type  of  terrain" 

(superior  *new-liap-liatener*) 

"terrain-type* ) 


(defun  get-terrain-random-data () 

(w:choose-variable-values 

'  <  (*obataoie-ratio*  (constraint  (lambda  (dl  d2  d3  value) 

(cond  ( {>  value  90)  "Too  Big") 
( (<  value  0)  "Error") 

(t  nil)))) 

("random-seed*  ifixnum)) 

(superior  *new-lisp-listener") 

(list  "obstacle-ratio*  "random-seed")! 


(defun  get-ditch-type  () 

( w ; chooae-Variable-valueo 

' ( (*ditch-type*  :menu-«liat  (("Add  Ditch"  (value  add-ditch) 

("No  Ditch"  (value  no-ditch)))) 

(label  "Choose  ditch  option" 
isuperior  *new-lisp-listener*) 

"ditch-type*) 
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(defun  get -ditch-width-location ( ) 

(wichooae-variable-values 

'  ( ("ditch-width*  constraint  (lambda  (dl  d2  d3  valua) 

(cond  ( (>  valua  7)  "Too  Big") 

((<  valua  3)  "Too  Small") 
<t  nil)))) 

("ditch-location*  (constraint  (lambda  (dl  &2  d3  valua) 

(eond  ( (>  value  32)  "Too  Big") 

((<  valua  IS)  "Too  Small") 
(t  nil))))) 

(superior  *new-lisp-listener*) 

(list  "ditch-width*  *ditoh-looation*) ) 


(defun  uaer-oM) 

(satf  *ok-£lag*  t) 

(w i choosa-variabla-valuas 
'  ( (*ok-£lag*  (boolean) ) 

(label  "Do  you  like  this  terrain?" 
isuparior  "new-lisp-listener") 
*ok~£lag*) 


(da£un  uaer-fila-nama ( ) 

< w ( choose -variable-values 
'  ( (*naw-terrain-£ile-name*  (Strinq)) 

(label  "Please  provide  the  output  tile  name." 
(superior  *new-lisp-listcner*) 
"new-terrain-file-nama") 


(dafun  uaar-aavaO 
(satf  *ok-£lag*  nil) 
(wjahooae-varitble-values 
'((*ok-flag*  "Sava-p"  (boolean)) 

(label  "Do  you  want  to  save  this  terrain?" 
(superior  *new-lisp-listener*) 

*ok~£lag*) 


(defun  move-and-ahape-liap-listener () 

(aetf  *soreen*  (eend  *tanninal-io*  (superior)) 

(setf  "screen-width*  (send  "screen*  (width)) 

(aetf  "screen-height*  (send  "screen*  (height)) 

(aetf  "new-lisp-listener*  (make-instance  'w(lisp-listener) ) 

(send  "new-lisp-listener^  (refresh) 

(send  "new-liap-listener*  (set-size 

(truncate  (*  1.0  "screen-width"))  (truncate  (*  0.2  "screen-height*))) 
(send  "new-lisp-listener"  s set-position 

0  (truncate  (*  0.8  "screen-height") ) ) 

(send  "new-lisp-listener*  :set-more-p  nil) 

(send  "new-lisp-listener"  (select)) 
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(defun  restore-lisp-listener () 

(send  *new-lisp-listener*  skill)) 


(defun  my-prlnt  (x) 

(print  x  *new-lisp-listener*) ) 


(defun  nsy-read-cher-no-heng  () 

.  (read-chsr-no-hen g  *n*w-lisp-liatener*)) 


i 
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III  Mod* : Common-Lisp;  Base: 10 

j******************** *********************************************** 

; 

i  vision-system  definition 


t 

j******************************* ************************************ 


(defflavor  vision-system  (owner) 
0 

: initable-instenoe-variables) 


(defmethod  (vision-system  iinitti) 
0 

) 


(defmethod 

) 


(vision-system  (scanning) 

0 


(defmethod  (vision-system  ipermitted-cell) 
(t-oell) 

(send  graph-terrain  ipermitted-oell  t-oell)) 


(defmethod  (vision-system  i terrain-point) 
(t-oell) 

(send  graph-terrain  i terrain-point  t-cell)) 
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